cpuinfoapp: Add SST discovery feature

Retrieve Intel Speed Select Technology (SST) configuration values for
all CPUs via PECI (OS-PCode mailbox). Each CPU may have up to three
Performance Profiles (PP), each with accompanying Base Frequency (BF)
information.

Discovery is started immediately, but if no CPUs are found or any
unexpected PECI error is encountered, discovery is aborted and scheduled
for periodic retries until complete.

The profile data is published on D-Bus using two predefined interfaces:
 - xyz.openbmc_project.Control.Processor.CurrentOperationConfig, which
   is implemented on each "cpu" object in the inventory, and contains
   mutable properties for OOB configuration (modifiying properties not
   supported yet).
 - xyz.openbmc_project.Inventory.Item.Cpu.OperationConfig, which is
   implemented on separate "config" objects and contains the readonly
   properties for each performance profile.

Tested:
 - Profiled performance of PECI operations via code instrumentation
   (takes ~2 min per CPU on ast2500 during BMC boot, ~2 sec during BMC idle).
 - Validated Redfish output against Linux driver using included python
   tool.
 - Injected PECI failures in code to test error handling, and tested
   with Linux OS idling on host to make sure WOP is working.

Change-Id: I0d8ae79655dfd2880cf3bae6abe600597740df7c
Signed-off-by: Jonathan Doman <jonathan.doman@intel.com>
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 55b026e..1b49606 100755
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -9,25 +9,22 @@
 # disable now to wa boost bug chriskohlhoff/asio#533
 # set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fno-rtti")
 
-include_directories (
-    ${CMAKE_CURRENT_SOURCE_DIR}/include/phosphor-dbus-interfaces
-)
 include_directories (${CMAKE_CURRENT_SOURCE_DIR}/include)
 link_directories (${DBUSINTERFACE_LIBRARIES})
 
 find_package (PkgConfig REQUIRED)
+
 pkg_check_modules (SYSTEMD libsystemd REQUIRED)
 include_directories (${SYSTEMD_INCLUDE_DIRS})
 link_directories (${SYSTEMD_LIBRARY_DIRS})
 
 # import sdbusplus
-find_package (PkgConfig REQUIRED)
 pkg_check_modules (SDBUSPLUSPLUS sdbusplus REQUIRED)
 include_directories (${SDBUSPLUSPLUS_INCLUDE_DIRS})
 link_directories (${SDBUSPLUSPLUS_LIBRARY_DIRS})
+find_program (SDBUSPLUSPLUS sdbus++)
 
 # phosphor-dbus-interfaces
-find_package (PkgConfig REQUIRED)
 pkg_check_modules (DBUSINTERFACE phosphor-dbus-interfaces REQUIRED)
 include_directories (${DBUSINTERFACE_INCLUDE_DIRS})
 link_directories (${DBUSINTERFACE_LIBRARY_DIRS})
@@ -47,19 +44,13 @@
 
 include_directories (${CMAKE_CURRENT_BINARY_DIR})
 
-find_package (PkgConfig REQUIRED)
-pkg_check_modules (SDBUSPLUSPLUS sdbusplus REQUIRED)
-include_directories (${SDBUSPLUSPLUS_INCLUDE_DIRS})
-link_directories (${SDBUSPLUSPLUS_LIBRARY_DIRS})
-find_program (SDBUSPLUSPLUS sdbus++)
-
 add_executable (smbiosmdrv2app ${SRC_FILES})
 target_link_libraries (smbiosmdrv2app ${SYSTEMD_LIBRARIES})
 target_link_libraries (smbiosmdrv2app ${DBUSINTERFACE_LIBRARIES})
 target_link_libraries (smbiosmdrv2app ${SDBUSPLUSPLUS_LIBRARIES})
 target_link_libraries (smbiosmdrv2app phosphor_logging)
 
-add_executable (cpuinfoapp src/cpuinfo_main.cpp)
+add_executable (cpuinfoapp src/cpuinfo_main.cpp src/speed_select.cpp)
 target_link_libraries (cpuinfoapp ${SYSTEMD_LIBRARIES})
 target_link_libraries (cpuinfoapp ${DBUSINTERFACE_LIBRARIES})
 target_link_libraries (cpuinfoapp ${SDBUSPLUSPLUS_LIBRARIES})
diff --git a/include/cpuinfo.hpp b/include/cpuinfo.hpp
index 796ed4c..5267c00 100644
--- a/include/cpuinfo.hpp
+++ b/include/cpuinfo.hpp
@@ -26,6 +26,8 @@
 static constexpr char const* cpuInfoObject = "xyz.openbmc_project.CPUInfo";
 static constexpr char const* cpuInfoPath = "/xyz/openbmc_project/CPUInfo";
 static constexpr char const* cpuInfoInterface = "xyz.openbmc_project.CPUInfo";
+static constexpr const char* cpuPath =
+    "/xyz/openbmc_project/inventory/system/chassis/motherboard/cpu";
 
 static constexpr const int peciCheckInterval = 10;
 
diff --git a/include/speed_select.hpp b/include/speed_select.hpp
new file mode 100644
index 0000000..aaa8b70
--- /dev/null
+++ b/include/speed_select.hpp
@@ -0,0 +1,40 @@
+// Copyright (c) 2020 Intel Corporation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//      http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+#pragma once
+
+#include <boost/asio/io_context.hpp>
+#include <sdbusplus/asio/connection.hpp>
+
+namespace cpu_info
+{
+namespace sst
+{
+
+/**
+ * Retrieve all SST configuration info for all discoverable CPUs, and publish
+ * the info on new D-Bus objects on the given bus connection.
+ *
+ * This function may block until all discovery is completed (many seconds), or
+ * it may schedule the work to be done at a later time (on the given ASIO
+ * context) if CPUs are not currently available, and may also schedule periodic
+ * work to be done after initial discovery is completed.
+ *
+ * @param[in,out]   ioc     ASIO IO context/service
+ * @param[in,out]   conn    D-Bus ASIO connection.
+ */
+void init(boost::asio::io_context& ioc,
+          const std::shared_ptr<sdbusplus::asio::connection>& conn);
+
+} // namespace sst
+} // namespace cpu_info
diff --git a/src/cpuinfo_main.cpp b/src/cpuinfo_main.cpp
index ffa603f..24bb858 100644
--- a/src/cpuinfo_main.cpp
+++ b/src/cpuinfo_main.cpp
@@ -15,6 +15,7 @@
 */
 
 #include "cpuinfo.hpp"
+#include "speed_select.hpp"
 
 #include <errno.h>
 #include <fcntl.h>
@@ -44,8 +45,6 @@
 namespace cpu_info
 {
 
-static constexpr const char* cpuPath =
-    "/xyz/openbmc_project/inventory/system/chassis/motherboard/cpu";
 static constexpr const char* cpuInterfaceName =
     "xyz.openbmc_project.Inventory.Decorator.Asset";
 static constexpr const char* cpuProcessName =
@@ -399,6 +398,8 @@
     sdbusplus::server::manager::manager objManager(
         bus, "/xyz/openbmc_project/inventory");
 
+    cpu_info::sst::init(io, conn);
+
     // Start the PECI check loop
     boost::asio::steady_timer peciWaitTimer(
         io, std::chrono::seconds(phosphor::cpu_info::peciCheckInterval));
diff --git a/src/speed_select.cpp b/src/speed_select.cpp
new file mode 100644
index 0000000..915f25c
--- /dev/null
+++ b/src/speed_select.cpp
@@ -0,0 +1,759 @@
+// Copyright (c) 2020 Intel Corporation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//      http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "speed_select.hpp"
+
+#include "cpuinfo.hpp"
+
+#include <peci.h>
+
+#include <boost/asio/steady_timer.hpp>
+#include <xyz/openbmc_project/Common/error.hpp>
+#include <xyz/openbmc_project/Control/Processor/CurrentOperatingConfig/server.hpp>
+#include <xyz/openbmc_project/Inventory/Item/Cpu/OperatingConfig/server.hpp>
+
+#include <iostream>
+#include <memory>
+#include <string>
+
+namespace cpu_info
+{
+namespace sst
+{
+
+class PECIError : public std::runtime_error
+{
+    using std::runtime_error::runtime_error;
+};
+
+constexpr uint64_t bit(int index)
+{
+    return (1ull << index);
+}
+
+constexpr int extendedModel(CPUModel model)
+{
+    return (model >> 16) & 0xF;
+}
+
+constexpr bool modelSupportsDiscovery(CPUModel model)
+{
+    return extendedModel(model) >= extendedModel(icx);
+}
+
+/**
+ * Construct a list of indexes of the set bits in the input value.
+ * E.g. fn(0x7A) -> {1,3,4,5,6}
+ *
+ * @param[in]   mask    Bitmask to convert.
+ *
+ * @return  List of bit indexes.
+ */
+static std::vector<uint32_t> convertMaskToList(std::bitset<64> mask)
+{
+    std::vector<uint32_t> bitList;
+    for (size_t i = 0; i < mask.size(); ++i)
+    {
+        if (mask.test(i))
+        {
+            bitList.push_back(i);
+        }
+    }
+    return bitList;
+}
+
+static bool checkPECIStatus(EPECIStatus libStatus, uint8_t completionCode)
+{
+    if (libStatus != PECI_CC_SUCCESS || completionCode != PECI_DEV_CC_SUCCESS)
+    {
+        std::cerr << "PECI command failed."
+                  << " Driver Status = " << libStatus << ","
+                  << " Completion Code = " << static_cast<int>(completionCode)
+                  << '\n';
+        return false;
+    }
+    return true;
+}
+
+/**
+ * Convenience RAII object for Wake-On-PECI (WOP) management, since PECI Config
+ * Local accesses to the OS Mailbox require the package to pop up to PC2. Also
+ * provides PCode OS Mailbox routine.
+ *
+ * Since multiple applications may be modifing WOP, we'll use this algorithm:
+ * Whenever a PECI command fails with associated error code, set WOP bit and
+ * retry command. Upon manager destruction, clear WOP bit only if we previously
+ * set it.
+ */
+struct PECIManager
+{
+    int peciAddress;
+    bool peciWoken;
+    CPUModel cpuModel;
+    int mbBus;
+
+    PECIManager(int address, CPUModel model) :
+        peciAddress(address), peciWoken(false), cpuModel(model)
+    {
+        mbBus = (model == icx) ? 14 : 31;
+    }
+
+    ~PECIManager()
+    {
+        // If we're being destroyed due to a PECIError, try to clear the mode
+        // bit, but catch and ignore any duplicate error it might raise to
+        // prevent termination.
+        try
+        {
+            if (peciWoken)
+            {
+                setWakeOnPECI(false);
+            }
+        }
+        catch (const PECIError& err)
+        {}
+    }
+
+    static bool isSleeping(EPECIStatus libStatus, uint8_t completionCode)
+    {
+        // PECI completion code defined in peci-ioctl.h which is not available
+        // for us to include.
+        constexpr int PECI_DEV_CC_UNAVAIL_RESOURCE = 0x82;
+        // Observed library returning DRIVER_ERR for reads and TIMEOUT for
+        // writes while PECI is sleeping. Either way, the completion code from
+        // PECI client should be reliable indicator of need to set WOP.
+        return libStatus != PECI_CC_SUCCESS &&
+               completionCode == PECI_DEV_CC_UNAVAIL_RESOURCE;
+    }
+
+    /**
+     * Send a single PECI PCS write to modify the Wake-On-PECI mode bit
+     */
+    void setWakeOnPECI(bool enable)
+    {
+        uint8_t completionCode;
+        EPECIStatus libStatus =
+            peci_WrPkgConfig(peciAddress, 5, enable ? 1 : 0, 0,
+                             sizeof(uint32_t), &completionCode);
+        if (!checkPECIStatus(libStatus, completionCode))
+        {
+            throw PECIError("Failed to set Wake-On-PECI mode bit");
+        }
+
+        if (enable)
+        {
+            peciWoken = true;
+        }
+    }
+
+    // PCode OS Mailbox interface register locations
+    static constexpr int mbSegment = 0;
+    static constexpr int mbDevice = 30;
+    static constexpr int mbFunction = 1;
+    static constexpr int mbDataReg = 0xA0;
+    static constexpr int mbInterfaceReg = 0xA4;
+    static constexpr int mbRegSize = sizeof(uint32_t);
+
+    enum class MailboxStatus
+    {
+        NoError = 0x0,
+        InvalidCommand = 0x1,
+        IllegalData = 0x16
+    };
+
+    /**
+     * Send a single Write PCI Config Local command, targeting the PCU CR1
+     * register block.
+     *
+     * @param[in]   regAddress  PCI Offset of register.
+     * @param[in]   data        Data to write.
+     */
+    void wrMailboxReg(uint16_t regAddress, uint32_t data)
+    {
+        uint8_t completionCode;
+        bool tryWaking = true;
+        while (true)
+        {
+            EPECIStatus libStatus = peci_WrEndPointPCIConfigLocal(
+                peciAddress, mbSegment, mbBus, mbDevice, mbFunction, regAddress,
+                mbRegSize, data, &completionCode);
+            if (tryWaking && isSleeping(libStatus, completionCode))
+            {
+                setWakeOnPECI(true);
+                tryWaking = false;
+                continue;
+            }
+            else if (!checkPECIStatus(libStatus, completionCode))
+            {
+                throw PECIError("Failed to write mailbox reg");
+            }
+            break;
+        }
+    }
+
+    /**
+     * Send a single Read PCI Config Local command, targeting the PCU CR1
+     * register block.
+     *
+     * @param[in]   regAddress  PCI offset of register.
+     *
+     * @return  Register value
+     */
+    uint32_t rdMailboxReg(uint16_t regAddress)
+    {
+        uint8_t completionCode;
+        uint32_t outputData;
+        bool tryWaking = true;
+        while (true)
+        {
+            EPECIStatus libStatus = peci_RdEndPointConfigPciLocal(
+                peciAddress, mbSegment, mbBus, mbDevice, mbFunction, regAddress,
+                mbRegSize, reinterpret_cast<uint8_t*>(&outputData),
+                &completionCode);
+            if (tryWaking && isSleeping(libStatus, completionCode))
+            {
+                setWakeOnPECI(true);
+                tryWaking = false;
+                continue;
+            }
+            if (!checkPECIStatus(libStatus, completionCode))
+            {
+                throw PECIError("Failed to read mailbox reg");
+            }
+            break;
+        }
+        return outputData;
+    }
+
+    /**
+     * Send command on PCode OS Mailbox interface.
+     *
+     * @param[in]   command     Main command ID.
+     * @param[in]   subCommand  Sub command ID.
+     * @param[in]   inputData   Data to put in mailbox. Is always written, but
+     *                          will be ignored by PCode if command is a
+     *                          "getter".
+     * @param[out]  responseCode    Optional parameter to receive the
+     *                              mailbox-level response status. If null, a
+     *                              PECIError will be thrown for error status.
+     *
+     * @return  Data returned in mailbox. Value is undefined if command is a
+     *          "setter".
+     */
+    uint32_t sendPECIOSMailboxCmd(uint8_t command, uint8_t subCommand,
+                                  uint32_t inputData = 0,
+                                  MailboxStatus* responseCode = nullptr)
+    {
+        // The simple mailbox algorithm just says to wait until the busy bit
+        // is clear, but we'll give up after 10 tries. It's arbitrary but that's
+        // quite long wall clock time.
+        constexpr int mbRetries = 10;
+        constexpr uint32_t mbBusyBit = bit(31);
+
+        // Wait until RUN_BUSY == 0
+        int attempts = mbRetries;
+        while ((rdMailboxReg(mbInterfaceReg) & mbBusyBit) == 1 &&
+               --attempts > 0)
+            ;
+        if (attempts == 0)
+        {
+            throw PECIError("OS Mailbox failed to become free");
+        }
+
+        // Write required command specific input data to data register
+        wrMailboxReg(mbDataReg, inputData);
+
+        // Write required command specific command/sub-command values and set
+        // RUN_BUSY bit in interface register.
+        uint32_t interfaceReg =
+            mbBusyBit | (static_cast<uint32_t>(subCommand) << 8) | command;
+        wrMailboxReg(mbInterfaceReg, interfaceReg);
+
+        // Wait until RUN_BUSY == 0
+        attempts = mbRetries;
+        do
+        {
+            interfaceReg = rdMailboxReg(mbInterfaceReg);
+        } while ((interfaceReg & mbBusyBit) == 1 && --attempts > 0);
+        if (attempts == 0)
+        {
+            throw PECIError("OS Mailbox failed to return");
+        }
+
+        // Read command return status or error code from interface register
+        auto status = static_cast<MailboxStatus>(interfaceReg & 0xFF);
+        if (responseCode != nullptr)
+        {
+            *responseCode = status;
+        }
+        else if (status != MailboxStatus::NoError)
+        {
+            throw PECIError(std::string("OS Mailbox returned with error: ") +
+                            std::to_string(static_cast<int>(status)));
+        }
+
+        // Read command return data from the data register
+        return rdMailboxReg(mbDataReg);
+    }
+};
+
+/**
+ * Base class for set of PECI OS Mailbox commands.
+ * Constructing it runs the command and stores the value for use by derived
+ * class accessor methods.
+ */
+template <uint8_t subcommand>
+struct OsMailboxCommand
+{
+    uint32_t value;
+    PECIManager::MailboxStatus status;
+    /**
+     * Construct the command object with required PECI address and up to 4
+     * optional 1-byte input data parameters.
+     */
+    OsMailboxCommand(PECIManager& pm, uint8_t param1 = 0, uint8_t param2 = 0,
+                     uint8_t param3 = 0, uint8_t param4 = 0)
+    {
+        uint32_t param =
+            (param4 << 24) | (param3 << 16) | (param2 << 8) | param1;
+        value = pm.sendPECIOSMailboxCmd(0x7F, subcommand, param, &status);
+    }
+
+    /** Return whether the mailbox status indicated success or not. */
+    bool success() const
+    {
+        return status == PECIManager::MailboxStatus::NoError;
+    }
+};
+
+/**
+ * Macro to define a derived class accessor method.
+ *
+ * @param[in]   type    Return type of accessor method.
+ * @param[in]   name    Name of accessor method.
+ * @param[in]   hibit   Most significant bit of field to access.
+ * @param[in]   lobit   Least significant bit of field to access.
+ */
+#define FIELD(type, name, hibit, lobit)                                        \
+    type name() const                                                          \
+    {                                                                          \
+        return (value >> lobit) & (bit(hibit - lobit + 1) - 1);                \
+    }
+
+struct GetLevelsInfo : OsMailboxCommand<0x0>
+{
+    using OsMailboxCommand::OsMailboxCommand;
+    FIELD(bool, enabled, 31, 31)
+    FIELD(bool, lock, 24, 24)
+    FIELD(int, currentConfigTdpLevel, 23, 16)
+    FIELD(int, configTdpLevels, 15, 8)
+    FIELD(int, version, 7, 0)
+};
+
+struct GetConfigTdpControl : OsMailboxCommand<0x1>
+{
+    using OsMailboxCommand::OsMailboxCommand;
+    FIELD(bool, pbfEnabled, 17, 17);
+    FIELD(bool, factEnabled, 16, 16);
+    FIELD(bool, pbfSupport, 1, 1);
+    FIELD(bool, factSupport, 0, 0);
+};
+
+struct GetTdpInfo : OsMailboxCommand<0x3>
+{
+    using OsMailboxCommand::OsMailboxCommand;
+    FIELD(int, tdpRatio, 23, 16);
+    FIELD(int, pkgTdp, 14, 0);
+};
+
+struct GetCoreMask : OsMailboxCommand<0x6>
+{
+    using OsMailboxCommand::OsMailboxCommand;
+    FIELD(uint32_t, coresMask, 31, 0);
+};
+
+struct GetTurboLimitRatios : OsMailboxCommand<0x7>
+{
+    using OsMailboxCommand::OsMailboxCommand;
+};
+
+struct GetRatioInfo : OsMailboxCommand<0xC>
+{
+    using OsMailboxCommand::OsMailboxCommand;
+    FIELD(int, pm, 31, 24);
+    FIELD(int, pn, 23, 16);
+    FIELD(int, p1, 15, 8);
+    FIELD(int, p0, 7, 0);
+};
+
+struct GetTjmaxInfo : OsMailboxCommand<0x5>
+{
+    using OsMailboxCommand::OsMailboxCommand;
+    FIELD(int, tProchot, 7, 0);
+};
+
+struct PbfGetCoreMaskInfo : OsMailboxCommand<0x20>
+{
+    using OsMailboxCommand::OsMailboxCommand;
+    FIELD(uint32_t, p1HiCoreMask, 31, 0);
+};
+
+struct PbfGetP1HiP1LoInfo : OsMailboxCommand<0x21>
+{
+    using OsMailboxCommand::OsMailboxCommand;
+    FIELD(int, p1Hi, 15, 8);
+    FIELD(int, p1Lo, 7, 0);
+};
+
+using BaseCurrentOperatingConfig =
+    sdbusplus::server::object_t<sdbusplus::xyz::openbmc_project::Control::
+                                    Processor::server::CurrentOperatingConfig>;
+
+using BaseOperatingConfig =
+    sdbusplus::server::object_t<sdbusplus::xyz::openbmc_project::Inventory::
+                                    Item::Cpu::server::OperatingConfig>;
+
+class OperatingConfig : public BaseOperatingConfig
+{
+  public:
+    std::string path;
+    int level;
+
+  public:
+    using BaseOperatingConfig::BaseOperatingConfig;
+    OperatingConfig(sdbusplus::bus::bus& bus, int level_, std::string path_) :
+        BaseOperatingConfig(bus, path_.c_str(), action::defer_emit),
+        path(std::move(path_)), level(level_)
+    {}
+};
+
+class CPUConfig : public BaseCurrentOperatingConfig
+{
+  private:
+    /** Objects describing all available SST configs - not modifiable. */
+    std::vector<std::unique_ptr<OperatingConfig>> availConfigs;
+    sdbusplus::bus::bus& bus;
+    int peciAddress;
+    std::string path; ///< D-Bus object path
+    CPUModel cpuModel;
+    int currentLevel;
+    bool bfEnabled;
+
+  public:
+    CPUConfig(sdbusplus::bus::bus& bus_, int index, CPUModel model) :
+        BaseCurrentOperatingConfig(bus_, generatePath(index).c_str(),
+                                   action::defer_emit),
+        bus(bus_), path(generatePath(index))
+    {
+        peciAddress = index + MIN_CLIENT_ADDR;
+        cpuModel = model;
+
+        // For now, read level and SST-BF status just once at start. Will be
+        // done dynamically in handlers in future commit.
+        PECIManager pm(peciAddress, cpuModel);
+        currentLevel = GetLevelsInfo(pm).currentConfigTdpLevel();
+        bfEnabled = GetConfigTdpControl(pm, currentLevel).pbfEnabled();
+    }
+
+    //
+    // D-Bus Property Overrides
+    //
+
+    sdbusplus::message::object_path appliedConfig() const override
+    {
+        return generateConfigPath(currentLevel);
+    }
+
+    bool baseSpeedPriorityEnabled() const override
+    {
+        return bfEnabled;
+    }
+
+    sdbusplus::message::object_path
+        appliedConfig(sdbusplus::message::object_path /* value */) override
+    {
+        throw sdbusplus::xyz::openbmc_project::Common::Error::NotAllowed();
+        // return value not used
+        return sdbusplus::message::object_path();
+    }
+
+    bool baseSpeedPriorityEnabled(bool /* value */) override
+    {
+        throw sdbusplus::xyz::openbmc_project::Common::Error::NotAllowed();
+        // return value not used
+        return false;
+    }
+
+    //
+    // Additions
+    //
+
+    OperatingConfig& newConfig(int level)
+    {
+        availConfigs.emplace_back(std::make_unique<OperatingConfig>(
+            bus, level, generateConfigPath(level)));
+        return *availConfigs.back();
+    }
+
+    std::string generateConfigPath(int level) const
+    {
+        return path + "/config" + std::to_string(level);
+    }
+
+    /**
+     * Emit the interface added signals which were deferred. This is required
+     * for ObjectMapper to pick up the objects.
+     */
+    void finalize()
+    {
+        emit_added();
+        for (auto& config : availConfigs)
+        {
+            config->emit_added();
+        }
+    }
+
+    static std::string generatePath(int index)
+    {
+        return phosphor::cpu_info::cpuPath + std::to_string(index);
+    }
+};
+
+/**
+ * Retrieve the SST parameters for a single config and fill the values into the
+ * properties on the D-Bus interface.
+ *
+ * @param[in,out]   peciManager     PECI context to use.
+ * @param[in]       level           Config TDP level to retrieve.
+ * @param[out]      config          D-Bus interface to update.
+ * @param[in]       trlCores        Turbo ratio limit core ranges from MSR
+ *                                  0x1AE. This is constant across all configs
+ *                                  in a CPU.
+ */
+static void getSingleConfig(PECIManager& peciManager, int level,
+                            OperatingConfig& config, uint64_t trlCores)
+{
+    constexpr int mhzPerRatio = 100;
+
+    // PowerLimit <= GET_TDP_INFO.PKG_TDP
+    config.powerLimit(GetTdpInfo(peciManager, level).pkgTdp());
+
+    // AvailableCoreCount <= GET_CORE_MASK.CORES_MASK
+    uint64_t coreMaskLo = GetCoreMask(peciManager, level, 0).coresMask();
+    uint64_t coreMaskHi = GetCoreMask(peciManager, level, 1).coresMask();
+    std::bitset<64> coreMask = (coreMaskHi << 32) | coreMaskLo;
+    config.availableCoreCount(coreMask.count());
+
+    // BaseSpeed <= GET_RATIO_INFO.P1
+    GetRatioInfo getRatioInfo(peciManager, level);
+    config.baseSpeed(getRatioInfo.p1() * mhzPerRatio);
+
+    // MaxSpeed <= GET_RATIO_INFO.P0
+    config.maxSpeed(getRatioInfo.p0() * mhzPerRatio);
+
+    // MaxJunctionTemperature <= GET_TJMAX_INFO.T_PROCHOT
+    config.maxJunctionTemperature(GetTjmaxInfo(peciManager, level).tProchot());
+
+    // Construct BaseSpeedPrioritySettings
+    GetConfigTdpControl getConfigTdpControl(peciManager, level);
+    std::vector<std::tuple<uint32_t, std::vector<uint32_t>>> baseSpeeds;
+    if (getConfigTdpControl.pbfSupport())
+    {
+        coreMaskLo = PbfGetCoreMaskInfo(peciManager, level, 0).p1HiCoreMask();
+        coreMaskHi = PbfGetCoreMaskInfo(peciManager, level, 1).p1HiCoreMask();
+        std::bitset<64> hiFreqCoreMask = (coreMaskHi << 32) | coreMaskLo;
+
+        std::vector<uint32_t> hiFreqCoreList, loFreqCoreList;
+        hiFreqCoreList = convertMaskToList(hiFreqCoreMask);
+        loFreqCoreList = convertMaskToList(coreMask & ~hiFreqCoreMask);
+
+        PbfGetP1HiP1LoInfo pbfGetP1HiP1LoInfo(peciManager, level);
+        baseSpeeds = {
+            {pbfGetP1HiP1LoInfo.p1Hi() * mhzPerRatio, hiFreqCoreList},
+            {pbfGetP1HiP1LoInfo.p1Lo() * mhzPerRatio, loFreqCoreList}};
+    }
+    config.baseSpeedPrioritySettings(baseSpeeds);
+
+    // Construct TurboProfile
+    std::vector<std::tuple<uint32_t, size_t>> turboSpeeds;
+
+    // Only read the SSE ratios (don't need AVX2/AVX512).
+    uint64_t limitRatioLo = GetTurboLimitRatios(peciManager, level, 0, 0).value;
+    uint64_t limitRatioHi = GetTurboLimitRatios(peciManager, level, 1, 0).value;
+    uint64_t limitRatios = (limitRatioHi << 32) | limitRatioLo;
+
+    constexpr int maxTFBuckets = 8;
+    for (int i = 0; i < maxTFBuckets; ++i)
+    {
+        size_t bucketCount = trlCores & 0xFF;
+        int bucketSpeed = limitRatios & 0xFF;
+        if (bucketCount != 0 && bucketSpeed != 0)
+        {
+            turboSpeeds.push_back({bucketSpeed * mhzPerRatio, bucketCount});
+        }
+        trlCores >>= 8;
+        limitRatios >>= 8;
+    }
+    config.turboProfile(turboSpeeds);
+}
+
+/**
+ * Retrieve all SST configuration info for all discoverable CPUs, and publish
+ * the info on new D-Bus objects on the given bus connection.
+ *
+ * @param[out]  cpuList     List to append info about discovered CPUs,
+ *                          including pointers to D-Bus objects to keep them
+ *                          alive. No items may be added to list in case host
+ *                          system is powered off and no CPUs are accessible.
+ * @param[in,out]   conn    D-Bus ASIO connection.
+ *
+ * @throw PECIError     A PECI command failed on a CPU which had previously
+ *                      responded to a command.
+ */
+static void
+    discoverCPUsAndConfigs(std::vector<std::unique_ptr<CPUConfig>>& cpuList,
+                           sdbusplus::asio::connection& conn)
+{
+    for (int i = MIN_CLIENT_ADDR; i <= MAX_CLIENT_ADDR; ++i)
+    {
+        // We could possibly check D-Bus for CPU presence and model, but PECI is
+        // 10x faster and so much simpler.
+        uint8_t cc, stepping;
+        CPUModel cpuModel;
+        EPECIStatus status = peci_GetCPUID(i, &cpuModel, &stepping, &cc);
+        if (status != PECI_CC_SUCCESS || cc != PECI_DEV_CC_SUCCESS ||
+            !modelSupportsDiscovery(cpuModel))
+        {
+            continue;
+        }
+
+        PECIManager peciManager(i, cpuModel);
+
+        // Continue if processor does not support SST-PP
+        GetLevelsInfo getLevelsInfo(peciManager);
+        if (!getLevelsInfo.enabled())
+        {
+            continue;
+        }
+
+        // Generate D-Bus object path for this processor.
+        int cpuIndex = i - MIN_CLIENT_ADDR;
+
+        // Read the Turbo Ratio Limit Cores MSR which is used to generate the
+        // Turbo Profile for each profile. This is a package scope MSR, so just
+        // read thread 0.
+        uint64_t trlCores;
+        status = peci_RdIAMSR(i, 0, 0x1AE, &trlCores, &cc);
+        if (!checkPECIStatus(status, cc))
+        {
+            throw PECIError("Failed to read TRL MSR");
+        }
+
+        // Create the per-CPU configuration object
+        // The server::object_t wrapper does not have a constructor which passes
+        // along property initializing values, so instead we need to tell it to
+        // defer emitting InterfacesAdded. If we emit the object added signal
+        // with an invalid object_path value, dbus-broker will kick us off the
+        // bus and we'll crash.
+        cpuList.emplace_back(
+            std::make_unique<CPUConfig>(conn, cpuIndex, cpuModel));
+        CPUConfig& cpu = *cpuList.back();
+
+        bool foundCurrentLevel = false;
+
+        for (int level = 0; level <= getLevelsInfo.configTdpLevels(); ++level)
+        {
+            // levels 1 and 2 are legacy/deprecated, originally used for AVX
+            // license pre-granting. They may be reused for more levels in
+            // future generations.
+            // We can check if they are supported by running any command for
+            // this level and checking the mailbox return status.
+            GetConfigTdpControl tdpControl(peciManager, level);
+            if (!tdpControl.success())
+            {
+                continue;
+            }
+
+            getSingleConfig(peciManager, level, cpu.newConfig(level), trlCores);
+
+            if (level == getLevelsInfo.currentConfigTdpLevel())
+            {
+                foundCurrentLevel = true;
+            }
+        }
+
+        if (!foundCurrentLevel)
+        {
+            // In case we didn't encounter a PECI error, but also didn't find
+            // the config which is supposedly applied, we won't be able to
+            // populate the CurrentOperatingConfig so we have to remove this CPU
+            // from consideration.
+            std::cerr << "CPU " << cpuIndex
+                      << " claimed SST support but invalid configs\n";
+            cpuList.pop_back();
+            continue;
+        }
+
+        cpu.finalize();
+    }
+}
+
+void init(boost::asio::io_context& ioc,
+          const std::shared_ptr<sdbusplus::asio::connection>& conn)
+{
+    static boost::asio::steady_timer peciRetryTimer(ioc);
+    static std::vector<std::unique_ptr<CPUConfig>> cpus;
+    static int peciErrorCount = 0;
+
+    try
+    {
+        discoverCPUsAndConfigs(cpus, *conn);
+        peciErrorCount = 0;
+    }
+    catch (const PECIError& err)
+    {
+        std::cerr << "PECI Error: " << err.what() << '\n';
+        // Drop any created interfaces to avoid presenting incomplete info
+        cpus.clear();
+
+        // In case of repeated failure to finish discovery, turn off this
+        // feature altogether. Possible cause is that the CPU model does not
+        // actually support the necessary mailbox commands.
+        if (++peciErrorCount >= 50)
+        {
+            std::cerr << "Aborting SST discovery\n";
+            return;
+        }
+
+        std::cerr << "Retrying SST discovery later\n";
+    }
+
+    // Retry later if no CPUs were available, or there was a PECI error.
+    // TODO: if there were cpus but none supported sst, stop trying.
+    if (cpus.empty())
+    {
+        peciRetryTimer.expires_after(std::chrono::seconds(10));
+        peciRetryTimer.async_wait([&ioc, conn](boost::system::error_code ec) {
+            if (ec)
+            {
+                std::cerr << "SST PECI Retry Timer failed: " << ec << '\n';
+                return;
+            }
+            init(ioc, conn);
+        });
+    }
+}
+
+} // namespace sst
+} // namespace cpu_info
diff --git a/tools/sst-compare-redfish-os.py b/tools/sst-compare-redfish-os.py
new file mode 100755
index 0000000..03fcf20
--- /dev/null
+++ b/tools/sst-compare-redfish-os.py
@@ -0,0 +1,207 @@
+#!/usr/bin/env python3
+
+# This tool runs on the host CPU and gathers all SST related configuration from
+# the BMC (Redfish) and from the linux driver, and compares them to catch any
+# errors or disagreement. Only required arguments are the details to start a
+# Redfish session.
+#
+# This was tested running on a live Arch Linux ISO environment. Any Linux
+# installation should work, but best to get the latest tools and kernel driver.
+#
+# Required dependencies:
+# * DMTF's redfish python library. This is available in pip.
+# * intel-speed-select tool from the kernel source tree
+#   (tools/power/x86/intel-speed-select), and available in the PATH.
+
+import redfish
+
+import argparse
+import json
+import re
+import subprocess
+import sys
+
+linux_cpu_map = dict()
+success = True
+
+def get_linux_output():
+    cmd = "/usr/bin/env intel-speed-select --debug --format json perf-profile info".split()
+    process = subprocess.run(cmd, capture_output=True, text=True)
+    process.check_returncode()
+    result = json.loads(process.stderr)
+
+    global linux_cpu_map
+    linux_cpu_map = dict()
+    for line in process.stdout.split('\n'):
+        match = re.search("logical_cpu:(\d+).*punit_core:(\d+)", line)
+        if not match:
+            continue
+        logical_thread = int(match.group(1))
+        physical_core = int(match.group(2))
+        linux_cpu_map[logical_thread] = physical_core
+
+    cmd = "/usr/bin/env intel-speed-select --format json perf-profile get-config-current-level".split()
+    process = subprocess.run(cmd, capture_output=True, text=True)
+    current_level = json.loads(process.stderr)
+
+    for proc, data in current_level.items():
+        result[proc].update(data)
+
+    return result
+
+
+def compare(redfish_val, linux_val, description):
+    err = ""
+    if redfish_val != linux_val:
+        err = "!! MISMATCH !!"
+        global success
+        success = False
+    print(f"{description}: {err}")
+    print(f"  Redfish: {redfish_val}")
+    print(f"  Linux: {linux_val}")
+
+
+def get_linux_package(linux_data, redfish_id):
+    match = re.match("cpu(\d+)", redfish_id)
+    if not match:
+        raise RuntimeError(f"Redfish CPU name is unexpected: {redfish_id}")
+    num = match.group(1)
+    matching_keys = []
+    for key in linux_data.keys():
+        if re.match(f"^package-{num}:.*", key):
+            matching_keys.append(key)
+    if len(matching_keys) != 1:
+        raise RuntimeError(f"Unexpected number of matching linux objects for {redfish_id}")
+    return linux_data[matching_keys[0]]
+
+
+def compare_config(redfish_config, linux_config):
+    print(f"--Checking {redfish_config['Id']}--")
+    compare(redfish_config["BaseSpeedMHz"], int(linux_config["base-frequency(MHz)"]), "Base Speed")
+
+    actual_hp_p1 = actual_lp_p1 = 0
+    actual_hp_cores = set()
+    for bf in redfish_config["BaseSpeedPrioritySettings"]:
+        if not actual_hp_p1 or bf["BaseSpeedMHz"] > actual_hp_p1:
+            actual_hp_p1 = bf["BaseSpeedMHz"]
+            actual_hp_cores = set(bf["CoreIDs"])
+        if not actual_lp_p1 or bf["BaseSpeedMHz"] < actual_lp_p1:
+            actual_lp_p1 = bf["BaseSpeedMHz"]
+
+    exp_hp_p1 = exp_lp_p1 = 0
+    exp_hp_cores = set()
+    if "speed-select-base-freq-properties" in linux_config:
+        exp_bf_props = linux_config["speed-select-base-freq-properties"]
+        exp_hp_p1 = int(exp_bf_props["high-priority-base-frequency(MHz)"])
+        exp_hp_cores = set(map(lambda x: linux_cpu_map[x],
+                              map(int, exp_bf_props["high-priority-cpu-list"].split(","))))
+        exp_lp_p1 = int(exp_bf_props["low-priority-base-frequency(MHz)"])
+
+    compare(actual_hp_p1, exp_hp_p1, "SST-BF High Priority P1 Freq")
+    compare(actual_hp_cores, exp_hp_cores, "SST-BF High Priority Core List")
+    compare(actual_lp_p1, exp_lp_p1, "SST-BF Low Priority P1 Freq")
+
+
+    compare(redfish_config["MaxJunctionTemperatureCelsius"],
+            int(linux_config["tjunction-max(C)"]),
+            "Junction Temperature")
+    compare(redfish_config["MaxSpeedMHz"],
+            int(linux_config["turbo-ratio-limits-sse"]["bucket-0"]["max-turbo-frequency(MHz)"]),
+            "SSE Max Turbo Speed")
+    compare(redfish_config["TDPWatts"],
+            int(linux_config["thermal-design-power(W)"]),
+            "TDP")
+    compare(redfish_config["TotalAvailableCoreCount"],
+            int(linux_config["enable-cpu-count"])//2,
+            "Enabled Core Count")
+
+    actual_turbo = [(x["ActiveCoreCount"], x["MaxSpeedMHz"]) for x in redfish_config["TurboProfile"]]
+    linux_turbo = linux_config["turbo-ratio-limits-sse"]
+    exp_turbo = []
+    for bucket_key in sorted(linux_turbo.keys()):
+        bucket = linux_turbo[bucket_key]
+        exp_turbo.append((int(bucket["core-count"]), int(bucket["max-turbo-frequency(MHz)"])))
+    compare(actual_turbo, exp_turbo, "SSE Turbo Profile")
+
+
+def get_level_from_config_id(config_id):
+    match = re.match("config(\d+)", config_id)
+    if not match:
+        raise RuntimeError(f"Invalid config name {config_id}")
+    return match.group(1)
+
+
+def main():
+    parser = argparse.ArgumentParser(description="Compare Redfish SST properties against Linux tools")
+    parser.add_argument("hostname")
+    parser.add_argument("--username", "-u", default="root")
+    parser.add_argument("--password", "-p", default="0penBmc")
+    args = parser.parse_args()
+
+    linux_data = get_linux_output()
+
+    bmc = redfish.redfish_client(base_url=f"https://{args.hostname}",
+            username=args.username, password=args.password)
+    bmc.login(auth="session")
+
+    # Load the ProcessorCollection
+    resp = json.loads(bmc.get("/redfish/v1/Systems/system/Processors").text)
+    for proc_member in resp["Members"]:
+        proc_resp = json.loads(bmc.get(proc_member["@odata.id"]).text)
+        proc_id = proc_resp["Id"]
+        print()
+        print(f"----Checking Processor {proc_id}----")
+
+        if proc_resp["Status"]["State"] == "Absent":
+            print("Not populated")
+            continue
+
+        # Get subset of intel-speed-select data which applies to this CPU
+        pkg_data = get_linux_package(linux_data, proc_id)
+
+        # Check currently applied config
+        applied_config = proc_resp["AppliedOperatingConfig"]["@odata.id"].split('/')[-1]
+        current_level = get_level_from_config_id(applied_config)
+        compare(current_level, pkg_data["get-config-current_level"], "Applied Config")
+
+        exp_cur_level_data = pkg_data[f"perf-profile-level-{current_level}"]
+
+        # Check whether SST-BF is enabled
+        bf_enabled = proc_resp["BaseSpeedPriorityState"].lower()
+        exp_bf_enabled = exp_cur_level_data["speed-select-base-freq"]
+        if exp_bf_enabled == "unsupported":
+            exp_bf_enabled = "disabled"
+        compare(bf_enabled, exp_bf_enabled, "SST-BF Enabled?")
+
+        # Check high speed core list
+        hscores = set(proc_resp["HighSpeedCoreIDs"])
+        exp_hscores = set()
+        if "speed-select-base-freq-properties" in exp_cur_level_data:
+            exp_hscores = exp_cur_level_data["speed-select-base-freq-properties"]["high-priority-cpu-list"]
+            exp_hscores = set([linux_cpu_map[int(x)] for x in exp_hscores.split(",")])
+        compare(hscores, exp_hscores, "High Speed Core List")
+
+        # Load the OperatingConfigCollection
+        resp = json.loads(bmc.get(proc_resp["OperatingConfigs"]["@odata.id"]).text)
+
+        # Check number of available configs
+        profile_keys = list(filter(lambda x: x.startswith("perf-profile-level"), pkg_data.keys()))
+        compare(resp["Members@odata.count"], int(len(profile_keys)), "Number of profiles")
+
+        for config_member in resp["Members"]:
+            # Load each OperatingConfig and compare all its contents
+            config_resp = json.loads(bmc.get(config_member["@odata.id"]).text)
+            level = get_level_from_config_id(config_resp["Id"])
+            exp_level_data = pkg_data[f"perf-profile-level-{level}"]
+            compare_config(config_resp, exp_level_data)
+
+    print()
+    if success:
+        print("Everything matched! :)")
+        return 0
+    else:
+        print("There were mismatches, please check output :(")
+        return 1
+
+if __name__ == "__main__":
+    sys.exit(main())
diff --git a/tools/sst-info.sh b/tools/sst-info.sh
new file mode 100755
index 0000000..6005948
--- /dev/null
+++ b/tools/sst-info.sh
@@ -0,0 +1,71 @@
+#!/bin/sh
+
+# Utility to print all SST data present on D-Bus.
+# Simply searches for all objects implementing known interfaces and prints out
+# the property values on those interfaces.
+
+BUSCTL='busctl'
+XYZ='xyz.openbmc_project'
+OBJECT_MAPPER="$XYZ.ObjectMapper /xyz/openbmc_project/object_mapper $XYZ.ObjectMapper"
+CPU_INTF="$XYZ.Control.Processor.CurrentOperatingConfig"
+CONFIG_INTF="$XYZ.Inventory.Item.Cpu.OperatingConfig"
+
+trim_quotes() {
+    trim_obj=${1%\"}
+    trim_obj=${trim_obj#\"}
+    echo $trim_obj
+}
+
+get_sub_tree_paths() {
+    resp=$($BUSCTL call $OBJECT_MAPPER GetSubTreePaths sias "$1" 0 "$2" "$3" \
+           | cut -d' ' -f3-)
+    for obj in $resp
+    do
+        trim_quotes $obj
+    done
+}
+
+get_service_from_object() {
+    trim_quotes $($BUSCTL call $OBJECT_MAPPER GetObject sas "$1" "$2" "$3" \
+                  | cut -d' ' -f3)
+}
+
+get_property_names() {
+    service=$1
+    object=$2
+    intf=$3
+    $BUSCTL introspect $service $object $intf \
+        | awk '/property/ {print substr($1, 2)}'
+}
+
+get_property() {
+    service=$1
+    object=$2
+    intf=$3
+    prop=$4
+    $BUSCTL get-property $service $object $intf $prop
+}
+
+
+cpu_paths=$(get_sub_tree_paths "/" 1 "$CPU_INTF")
+for cpu_path in $cpu_paths
+do
+    service=$(get_service_from_object $cpu_path 1 $CPU_INTF)
+    echo "Found SST on $cpu_path on $service"
+    for prop in $(get_property_names $service $cpu_path $CPU_INTF)
+    do
+        echo "  $prop: $(get_property $service $cpu_path $CPU_INTF $prop)"
+    done
+
+
+    profiles=$(get_sub_tree_paths "$cpu_path" 1 "$CONFIG_INTF")
+    for profile in $profiles
+    do
+        echo
+        echo "  Found Profile $profile"
+        for prop in $(get_property_names $service $profile $CONFIG_INTF)
+        do
+            echo "    $prop: $(get_property $service $profile $CONFIG_INTF $prop)"
+        done
+    done
+done