FeatureStore: Implement feature discovery and get/set logic

Implements the discovery, get, and set logic in FeatureStore.

Design-Doc: go/bmc-redfish-nvme-get-set-features
Google-Bug-Id: 405609473
Change-Id: If79b4f2656263a93f2a26b1cead3588ddda3bc49
Signed-off-by: Guangzong Chen <guangzong@google.com>
diff --git a/src/NVMeFeatureStore.cpp b/src/NVMeFeatureStore.cpp
new file mode 100644
index 0000000..b08ce56
--- /dev/null
+++ b/src/NVMeFeatureStore.cpp
@@ -0,0 +1,724 @@
+#include "NVMeFeatureStore.hpp"
+
+#include "nvme_features.pb.h"
+
+#include <endian.h>
+
+#include <boost/asio/async_result.hpp>
+#include <boost/asio/post.hpp>
+#include <phosphor-logging/lg2.hpp>
+
+#include <cstring>
+#include <iomanip>
+#include <map>
+#include <sstream>
+#include <tuple>
+#include <utility>
+#include <vector>
+
+static constexpr uint32_t nvmeGetFeaturesCapSaveable = 0x1;
+static constexpr uint32_t nvmeGetFeaturesCapChangeable = 0x4;
+
+struct FeatureMeta
+{
+    uint8_t fid;            ///< NVMe Feature Identifier
+    std::string protoClass; ///< Fully qualified Protobuf class name
+    FeatureScope scope;     ///< Applicability scope
+};
+
+/**
+ * @brief Map of P0 features defined in the design document.
+ */
+static const std::map<std::string, FeatureMeta> supportedFeatures = {
+    {"Arbitration",
+     {0x01, "google.gbmc.nvme.base.Arbitration", FeatureScope::Controller}},
+    {"PowerManagement",
+     {0x02, "google.gbmc.nvme.base.PowerManagement", FeatureScope::Controller}},
+    {"TemperatureThreshold",
+     {0x04, "google.gbmc.nvme.base.TemperatureThreshold",
+      FeatureScope::Controller}},
+    {"ErrorRecovery",
+     {0x05, "google.gbmc.nvme.base.ErrorRecovery", FeatureScope::Namespace}},
+    {"VolatileWriteCache",
+     {0x06, "google.gbmc.nvme.base.VolatileWriteCache",
+      FeatureScope::Controller}},
+    {"NumberOfQueues",
+     {0x07, "google.gbmc.nvme.base.NumberOfQueues", FeatureScope::Controller}},
+    {"InterruptCoalescing",
+     {0x08, "google.gbmc.nvme.base.InterruptCoalescing",
+      FeatureScope::Controller}},
+    {"InterruptVectorConfiguration",
+     {0x09, "google.gbmc.nvme.base.InterruptVectorConfiguration",
+      FeatureScope::Controller}},
+    {"WriteAtomicityNormal",
+     {0x0A, "google.gbmc.nvme.base.WriteAtomicityNormal",
+      FeatureScope::Controller}},
+    {"AsynchronousEventConfiguration",
+     {0x0B, "google.gbmc.nvme.base.AsynchronousEventConfiguration",
+      FeatureScope::Controller}},
+};
+
+template <typename T>
+static T parseProto(const std::vector<uint8_t>& data)
+{
+    T msg;
+    if (!msg.ParseFromArray(data.data(), static_cast<int>(data.size())))
+    {
+        throw sdbusplus::xyz::openbmc_project::Common::Error::InvalidArgument();
+    }
+    return msg;
+}
+
+// Helper for async_initiate
+static std::tuple<nvme_ex_ptr, uint32_t, std::vector<uint8_t>>
+    asyncAdminGetFeatures(boost::asio::io_context& io,
+                          const std::shared_ptr<NVMeMiIntf>& nvmeIntf,
+                          nvme_mi_ctrl_t ctrl,
+                          const NVMeMiIntf::GetFeaturesRequest& req,
+                          boost::asio::yield_context yield)
+{
+    return boost::asio::async_initiate<boost::asio::yield_context,
+                                       void(nvme_ex_ptr, uint32_t,
+                                            std::vector<uint8_t>)>(
+        [&io](auto&& callback, const std::shared_ptr<NVMeMiIntf>& nvmeIntf,
+              nvme_mi_ctrl_t ctrl, const NVMeMiIntf::GetFeaturesRequest& req) {
+        auto sharedCb = std::make_shared<std::decay_t<decltype(callback)>>(
+            std::forward<decltype(callback)>(callback));
+        nvmeIntf->adminGetFeatures(
+            ctrl, req,
+            [sharedCb, &io](nvme_ex_ptr ex, uint32_t resp,
+                            std::span<uint8_t> data) mutable {
+            std::vector<uint8_t> dataCopy(data.begin(), data.end());
+            boost::asio::post(io, [sharedCb = std::move(sharedCb), ex, resp,
+                                   dataCopy = std::move(dataCopy)]() mutable {
+                (*sharedCb)(ex, resp, dataCopy);
+            });
+        });
+    },
+        yield, nvmeIntf, ctrl, req);
+}
+
+static std::tuple<nvme_ex_ptr, uint32_t> asyncAdminSetFeatures(
+    boost::asio::io_context& io, const std::shared_ptr<NVMeMiIntf>& nvmeIntf,
+    nvme_mi_ctrl_t ctrl, const NVMeMiIntf::SetFeaturesRequest& req,
+    boost::asio::yield_context yield)
+{
+    return boost::asio::async_initiate<boost::asio::yield_context,
+                                       void(nvme_ex_ptr, uint32_t)>(
+        [&io](auto&& callback, const std::shared_ptr<NVMeMiIntf>& nvmeIntf,
+              nvme_mi_ctrl_t ctrl, const NVMeMiIntf::SetFeaturesRequest& req) {
+        auto sharedCb = std::make_shared<std::decay_t<decltype(callback)>>(
+            std::forward<decltype(callback)>(callback));
+        nvmeIntf->adminSetFeatures(
+            ctrl, req, [sharedCb, &io](nvme_ex_ptr ex, uint32_t resp) mutable {
+            boost::asio::post(io, [sharedCb = std::move(sharedCb), ex,
+                                   resp]() mutable { (*sharedCb)(ex, resp); });
+        });
+    },
+        yield, nvmeIntf, ctrl, req);
+}
+
+static std::tuple<nvme_ex_ptr, std::vector<uint8_t>> asyncAdminIdentify(
+    boost::asio::io_context& io, const std::shared_ptr<NVMeMiIntf>& nvmeIntf,
+    nvme_mi_ctrl_t ctrl, nvme_identify_cns cns, uint32_t nsid, uint16_t cntid,
+    boost::asio::yield_context yield)
+{
+    return boost::asio::async_initiate<boost::asio::yield_context,
+                                       void(nvme_ex_ptr, std::vector<uint8_t>)>(
+        [&io](auto&& callback, const std::shared_ptr<NVMeMiIntf>& nvmeIntf,
+              nvme_mi_ctrl_t ctrl, nvme_identify_cns cns, uint32_t nsid,
+              uint16_t cntid) {
+        auto sharedCb = std::make_shared<std::decay_t<decltype(callback)>>(
+            std::forward<decltype(callback)>(callback));
+        nvmeIntf->adminIdentify(
+            ctrl, cns, nsid, cntid,
+            [sharedCb, &io](nvme_ex_ptr ex, std::span<uint8_t> data) mutable {
+            std::vector<uint8_t> dataCopy(data.begin(), data.end());
+            boost::asio::post(io, [sharedCb = std::move(sharedCb), ex,
+                                   dataCopy = std::move(dataCopy)]() mutable {
+                (*sharedCb)(ex, dataCopy);
+            });
+        });
+    },
+        yield, nvmeIntf, ctrl, cns, nsid, cntid);
+}
+
+FeatureStore::FeatureStore(
+    boost::asio::io_context& io, sdbusplus::asio::object_server& objServer,
+    const std::shared_ptr<sdbusplus::asio::connection>& conn,
+    const std::string& objectPath, std::shared_ptr<NVMeMiIntf> nvmeIntf,
+    nvme_mi_ctrl_t ctrl, uint32_t nsid, FeatureScope scope) :
+    io(io), objServer(objServer), conn(conn), objectPath(objectPath),
+    nvmeIntf(std::move(nvmeIntf)), ctrl(ctrl), nsid(nsid), scope(scope)
+{}
+
+void FeatureStore::registerDbusInterface()
+{
+    dbusIntf = objServer.add_interface(objectPath,
+                                       "xyz.openbmc_project.NVMe.FeatureStore");
+
+    dbusIntf->register_property("ProtoLibraryVersion", std::string("2.0"),
+                                sdbusplus::asio::PropertyPermission::readOnly);
+    dbusIntf->register_property("FeatureCollection", featureCollection,
+                                sdbusplus::asio::PropertyPermission::readOnly);
+    dbusIntf->register_method(
+        "GetFeature",
+        [weakSelf = weak_from_this()](boost::asio::yield_context yield,
+                                      const std::string& featureName) {
+        auto self = weakSelf.lock();
+        if (!self)
+        {
+            throw sdbusplus::xyz::openbmc_project::Common::Error::Unavailable();
+        }
+        return self->getFeature(std::move(yield), featureName);
+    });
+
+    dbusIntf->register_method(
+        "SetFeature",
+        [weakSelf = weak_from_this()](boost::asio::yield_context yield,
+                                      const std::string& featureName,
+                                      const std::vector<uint8_t>& data) {
+        auto self = weakSelf.lock();
+        if (!self)
+        {
+            throw sdbusplus::xyz::openbmc_project::Common::Error::Unavailable();
+        }
+        self->setFeature(std::move(yield), featureName, data);
+    });
+
+    dbusIntf->initialize();
+}
+
+void FeatureStore::init(std::function<void(const std::error_code&)>&& cb)
+{
+    boost::asio::spawn(io,
+                       [self{shared_from_this()}, cb{std::move(cb)}](
+                           const boost::asio::yield_context& yield) mutable {
+        self->fetchIdentifyData(yield);
+        self->registerDbusInterface();
+        cb({});
+    });
+}
+
+void FeatureStore::fetchIdentifyData(const boost::asio::yield_context& yield)
+{
+    auto [ex, data] = asyncAdminIdentify(
+        io, nvmeIntf, ctrl, NVME_IDENTIFY_CNS_CTRL, NVME_NSID_NONE, 0, yield);
+
+    if (ex)
+    {
+        lg2::error(
+            "Identify Controller failed during FeatureStore init: {ERROR}",
+            "ERROR", ex->what());
+        discoverFeatures(yield);
+        return;
+    }
+
+    if (data.size() < sizeof(nvme_id_ctrl))
+    {
+        lg2::error("Identify Controller returned insufficient data");
+        discoverFeatures(yield);
+        return;
+    }
+    nvme_id_ctrl idCtrl = {};
+    std::memcpy(&idCtrl, data.data(), sizeof(nvme_id_ctrl));
+    vwcSupported = (idCtrl.vwc & NVME_CTRL_VWC_PRESENT) != 0;
+    oncs = le16toh(idCtrl.oncs);
+
+    if (!vwcSupported)
+    {
+        lg2::info("Volatile Write Cache (FID 0x06) not supported by hardware");
+    }
+
+    discoverFeatures(yield);
+}
+
+void FeatureStore::discoverFeatures(const boost::asio::yield_context& yield)
+{
+    bool oncsBit4 = (oncs & (1 << 4)) != 0;
+
+    featureCollection.clear();
+
+    std::vector<std::pair<std::string, FeatureMeta>> featuresToCheck;
+    for (const auto& [name, meta] : supportedFeatures)
+    {
+        if (meta.scope == scope)
+        {
+            if (name == "VolatileWriteCache" && !vwcSupported)
+            {
+                continue;
+            }
+            featuresToCheck.emplace_back(name, meta);
+        }
+    }
+
+    for (const auto& [name, meta] : featuresToCheck)
+    {
+        std::stringstream ss;
+        ss << "0x" << std::setfill('0') << std::setw(2) << std::hex
+           << static_cast<int>(meta.fid);
+        std::string fidStr = ss.str();
+
+        if (!oncsBit4)
+        {
+            featureCollection.emplace_back(name + "_Current", fidStr,
+                                           meta.protoClass);
+            continue;
+        }
+
+        NVMeMiIntf::GetFeaturesRequest args{};
+        args.fid = meta.fid;
+        args.sel = NVME_GET_FEATURES_SEL_SUPPORTED; // 011b
+        args.nsid = (scope == FeatureScope::Namespace) ? nsid : NVME_NSID_NONE;
+        args.cdw11 = 0;
+        args.data_len = 0;
+
+        auto [ex, cdw0, data] = asyncAdminGetFeatures(io, nvmeIntf, ctrl, args,
+                                                      yield);
+
+        if (ex)
+        {
+            lg2::error("Failed to query capabilities for FID {FID}: {ERROR}",
+                       "FID", meta.fid, "ERROR", ex->what());
+            featureCollection.emplace_back(name + "_Current", fidStr,
+                                           meta.protoClass);
+        }
+        else
+        {
+            featureCaps[meta.fid] = cdw0;
+
+            featureCollection.emplace_back(name + "_Current", fidStr,
+                                           meta.protoClass);
+
+            if ((cdw0 & nvmeGetFeaturesCapChangeable) != 0)
+            {
+                featureCollection.emplace_back(name + "_Default", fidStr,
+                                               meta.protoClass);
+            }
+
+            if ((cdw0 & nvmeGetFeaturesCapSaveable) != 0)
+            {
+                featureCollection.emplace_back(name + "_Saved", fidStr,
+                                               meta.protoClass);
+            }
+        }
+    }
+}
+
+std::vector<uint8_t> FeatureStore::getFeature(boost::asio::yield_context yield,
+                                              const std::string& featureName)
+{
+    size_t lastUnderscore = featureName.find_last_of('_');
+    if (lastUnderscore == std::string::npos)
+    {
+        throw sdbusplus::xyz::openbmc_project::Common::Error::InvalidArgument();
+    }
+
+    std::string baseName = featureName.substr(0, lastUnderscore);
+    std::string selectStr = featureName.substr(lastUnderscore + 1);
+
+    auto it = supportedFeatures.find(baseName);
+    if (it == supportedFeatures.end() || it->second.scope != scope)
+    {
+        throw sdbusplus::xyz::openbmc_project::Common::Error::InvalidArgument();
+    }
+
+    uint8_t fid = it->second.fid;
+    uint8_t sel = 0;
+    if (selectStr == "Current")
+    {
+        sel = 0;
+    }
+    else if (selectStr == "Default")
+    {
+        sel = 1;
+    }
+    else if (selectStr == "Saved")
+    {
+        sel = 2;
+    }
+    else
+    {
+        throw sdbusplus::xyz::openbmc_project::Common::Error::InvalidArgument();
+    }
+
+    NVMeMiIntf::GetFeaturesRequest args{};
+    args.fid = fid;
+    args.sel = sel;
+    args.nsid = (scope == FeatureScope::Namespace) ? nsid : NVME_NSID_NONE;
+    args.cdw11 = 0;
+    args.data_len = 0;
+
+    auto [ex, cdw0, data] = asyncAdminGetFeatures(io, nvmeIntf, ctrl, args,
+                                                  std::move(yield));
+
+    if (ex)
+    {
+        lg2::error("NVMe Get Features (FID {FID}) failed: {ERROR}", "FID", fid,
+                   "ERROR", ex->what());
+        throw sdbusplus::xyz::openbmc_project::Common::Error::Unavailable();
+    }
+
+    std::vector<uint8_t> protoData;
+
+    switch (fid)
+    {
+        case 0x01:
+        {
+            google::gbmc::nvme::base::Arbitration msg;
+            msg.set_arbitration_burst(cdw0 & 0xFF);
+            msg.set_low_priority_weight((cdw0 >> 8) & 0xFF);
+            msg.set_medium_priority_weight((cdw0 >> 16) & 0xFF);
+            msg.set_high_priority_weight((cdw0 >> 24) & 0xFF);
+            protoData.resize(msg.ByteSizeLong());
+            if (!msg.SerializeToArray(protoData.data(),
+                                      static_cast<int>(protoData.size())))
+            {
+                lg2::error("Failed to serialize Arbitration");
+            }
+            break;
+        }
+        case 0x02:
+        {
+            google::gbmc::nvme::base::PowerManagement msg;
+            msg.set_power_state(cdw0 & 0x1F);
+            msg.set_workload_hint((cdw0 >> 8) & 0x7);
+            protoData.resize(msg.ByteSizeLong());
+            if (!msg.SerializeToArray(protoData.data(),
+                                      static_cast<int>(protoData.size())))
+            {
+                lg2::error("Failed to serialize PowerManagement");
+            }
+            break;
+        }
+        case 0x04:
+        {
+            google::gbmc::nvme::base::TemperatureThreshold msg;
+            msg.set_temperature_threshold(cdw0 & 0xFFFF);
+            msg.set_threshold_temperature_select((cdw0 >> 16) & 0xF);
+            msg.set_threshold_type_select(
+                static_cast<google::gbmc::nvme::base::
+                                TemperatureThreshold_ThresholdType>(
+                    (cdw0 >> 20) & 0x3));
+            protoData.resize(msg.ByteSizeLong());
+            if (!msg.SerializeToArray(protoData.data(),
+                                      static_cast<int>(protoData.size())))
+            {
+                lg2::error("Failed to serialize TemperatureThreshold");
+            }
+            break;
+        }
+        case 0x05:
+        {
+            google::gbmc::nvme::base::ErrorRecovery msg;
+            msg.set_tler(cdw0 & 0xFFFF);
+            msg.set_dulbe_enable(
+                static_cast<google::gbmc::nvme::base::ErrorRecovery_State>(
+                    (cdw0 >> 16) & 0x1));
+            protoData.resize(msg.ByteSizeLong());
+            if (!msg.SerializeToArray(protoData.data(),
+                                      static_cast<int>(protoData.size())))
+            {
+                lg2::error("Failed to serialize ErrorRecovery");
+            }
+            break;
+        }
+        case 0x06:
+        {
+            google::gbmc::nvme::base::VolatileWriteCache msg;
+            msg.set_write_cache_enable(
+                static_cast<google::gbmc::nvme::base::VolatileWriteCache_State>(
+                    cdw0 & 0x1));
+            protoData.resize(msg.ByteSizeLong());
+            if (!msg.SerializeToArray(protoData.data(),
+                                      static_cast<int>(protoData.size())))
+            {
+                lg2::error("Failed to serialize VolatileWriteCache");
+            }
+            break;
+        }
+        case 0x07:
+        {
+            google::gbmc::nvme::base::NumberOfQueues msg;
+            msg.set_number_of_submission_queues(cdw0 & 0xFFFF);
+            msg.set_number_of_completion_queues((cdw0 >> 16) & 0xFFFF);
+            protoData.resize(msg.ByteSizeLong());
+            if (!msg.SerializeToArray(protoData.data(),
+                                      static_cast<int>(protoData.size())))
+            {
+                lg2::error("Failed to serialize NumberOfQueues");
+            }
+            break;
+        }
+        case 0x08:
+        {
+            google::gbmc::nvme::base::InterruptCoalescing msg;
+            msg.set_aggregation_threshold(cdw0 & 0xFF);
+            msg.set_aggregation_time((cdw0 >> 8) & 0xFF);
+            protoData.resize(msg.ByteSizeLong());
+            if (!msg.SerializeToArray(protoData.data(),
+                                      static_cast<int>(protoData.size())))
+            {
+                lg2::error("Failed to serialize Interrupt Coalescing");
+            }
+            break;
+        }
+        case 0x09:
+        {
+            google::gbmc::nvme::base::InterruptVectorConfiguration msg;
+            msg.set_interrupt_vector(cdw0 & 0xFFFF);
+            msg.set_coalescing_disable(
+                static_cast<google::gbmc::nvme::base::
+                                InterruptVectorConfiguration_CoalescingState>(
+                    (cdw0 >> 16) & 0x1));
+            protoData.resize(msg.ByteSizeLong());
+            if (!msg.SerializeToArray(protoData.data(),
+                                      static_cast<int>(protoData.size())))
+            {
+                lg2::error("Failed to serialize InterruptVectorConfiguration");
+            }
+            break;
+        }
+        case 0x0A:
+        {
+            google::gbmc::nvme::base::WriteAtomicityNormal msg;
+            msg.set_disable_normal(
+                static_cast<google::gbmc::nvme::base::
+                                WriteAtomicityNormal_NormalWriteAtomicityState>(
+                    cdw0 & 0x1));
+            protoData.resize(msg.ByteSizeLong());
+            if (!msg.SerializeToArray(protoData.data(),
+                                      static_cast<int>(protoData.size())))
+            {
+                lg2::error("Failed to serialize WriteAtomicityNormal");
+            }
+            break;
+        }
+        case 0x0B:
+        {
+            google::gbmc::nvme::base::AsynchronousEventConfiguration msg;
+            msg.set_smart_critical_warning_config(cdw0 & 0xFF);
+            protoData.resize(msg.ByteSizeLong());
+            if (!msg.SerializeToArray(protoData.data(),
+                                      static_cast<int>(protoData.size())))
+            {
+                lg2::error(
+                    "Failed to serialize AsynchronousEventConfiguration");
+            }
+            break;
+        }
+        default:
+            break;
+    }
+
+    return protoData;
+}
+
+void FeatureStore::setFeature(boost::asio::yield_context yield,
+                              const std::string& featureName,
+                              const std::vector<uint8_t>& data)
+{
+    size_t lastUnderscore = featureName.find_last_of('_');
+    if (lastUnderscore == std::string::npos)
+    {
+        throw sdbusplus::xyz::openbmc_project::Common::Error::InvalidArgument();
+    }
+
+    std::string baseName = featureName.substr(0, lastUnderscore);
+    std::string selectStr = featureName.substr(lastUnderscore + 1);
+
+    auto it = supportedFeatures.find(baseName);
+    if (it == supportedFeatures.end() || it->second.scope != scope)
+    {
+        throw sdbusplus::xyz::openbmc_project::Common::Error::InvalidArgument();
+    }
+
+    if (selectStr == "Default")
+    {
+        // Default endpoint is read-only
+        throw sdbusplus::xyz::openbmc_project::Common::Error::NotAllowed();
+    }
+    if (selectStr != "Current" && selectStr != "Saved")
+    {
+        throw sdbusplus::xyz::openbmc_project::Common::Error::InvalidArgument();
+    }
+
+    uint8_t fid = it->second.fid;
+    bool save = (selectStr == "Saved");
+
+    auto capIt = featureCaps.find(fid);
+    if (capIt != featureCaps.end())
+    {
+        uint32_t caps = capIt->second;
+        bool changeable = (caps & nvmeGetFeaturesCapChangeable) != 0;
+        bool saveable = (caps & nvmeGetFeaturesCapSaveable) != 0;
+
+        if (save && !saveable)
+        {
+            lg2::error("SetFeature: FID {FID} is not saveable per SEL=011b",
+                       "FID", lg2::hex, fid);
+            throw sdbusplus::xyz::openbmc_project::Common::Error::NotAllowed();
+        }
+
+        if (!save && !changeable)
+        {
+            lg2::error("SetFeature: FID {FID} is not changeable per SEL=011b",
+                       "FID", lg2::hex, fid);
+            throw sdbusplus::xyz::openbmc_project::Common::Error::NotAllowed();
+        }
+    }
+
+    uint32_t cdw11 = 0;
+
+    switch (fid)
+    {
+        case 0x01:
+        {
+            auto msg = parseProto<google::gbmc::nvme::base::Arbitration>(data);
+            if (msg.arbitration_burst() > 0xFF ||
+                msg.low_priority_weight() > 0xFF ||
+                msg.medium_priority_weight() > 0xFF ||
+                msg.high_priority_weight() > 0xFF)
+            {
+                throw sdbusplus::xyz::openbmc_project::Common::Error::
+                    InvalidArgument();
+            }
+            cdw11 = (msg.arbitration_burst() & 0xFF) |
+                    ((msg.low_priority_weight() & 0xFF) << 8) |
+                    ((msg.medium_priority_weight() & 0xFF) << 16) |
+                    ((msg.high_priority_weight() & 0xFF) << 24);
+            break;
+        }
+        case 0x02:
+        {
+            auto msg =
+                parseProto<google::gbmc::nvme::base::PowerManagement>(data);
+            if (msg.power_state() > 0x1F || msg.workload_hint() > 0x7)
+            {
+                throw sdbusplus::xyz::openbmc_project::Common::Error::
+                    InvalidArgument();
+            }
+            cdw11 = (msg.power_state() & 0x1F) |
+                    ((msg.workload_hint() & 0x7) << 8);
+            break;
+        }
+        case 0x04:
+        {
+            auto msg =
+                parseProto<google::gbmc::nvme::base::TemperatureThreshold>(
+                    data);
+            if (msg.temperature_threshold() > 0xFFFF ||
+                msg.threshold_temperature_select() > 0xF)
+            {
+                throw sdbusplus::xyz::openbmc_project::Common::Error::
+                    InvalidArgument();
+            }
+            cdw11 = (msg.temperature_threshold() & 0xFFFF) |
+                    ((msg.threshold_temperature_select() & 0xF) << 16) |
+                    ((static_cast<uint32_t>(msg.threshold_type_select()) & 0x3)
+                     << 20);
+            break;
+        }
+        case 0x05:
+        {
+            auto msg =
+                parseProto<google::gbmc::nvme::base::ErrorRecovery>(data);
+            if (msg.tler() > 0xFFFF)
+            {
+                throw sdbusplus::xyz::openbmc_project::Common::Error::
+                    InvalidArgument();
+            }
+            cdw11 = (msg.tler() & 0xFFFF) |
+                    ((static_cast<uint32_t>(msg.dulbe_enable()) & 0x1) << 16);
+            break;
+        }
+        case 0x06:
+        {
+            auto msg =
+                parseProto<google::gbmc::nvme::base::VolatileWriteCache>(data);
+            cdw11 = static_cast<uint32_t>(msg.write_cache_enable()) & 0x1;
+            break;
+        }
+        case 0x07:
+        {
+            auto msg =
+                parseProto<google::gbmc::nvme::base::NumberOfQueues>(data);
+            if (msg.number_of_submission_queues() > 0xFFFF ||
+                msg.number_of_completion_queues() > 0xFFFF)
+            {
+                throw sdbusplus::xyz::openbmc_project::Common::Error::
+                    InvalidArgument();
+            }
+            cdw11 = (msg.number_of_submission_queues() & 0xFFFF) |
+                    ((msg.number_of_completion_queues() & 0xFFFF) << 16);
+            break;
+        }
+        case 0x08:
+        {
+            auto msg =
+                parseProto<google::gbmc::nvme::base::InterruptCoalescing>(data);
+            if (msg.aggregation_threshold() > 0xFF ||
+                msg.aggregation_time() > 0xFF)
+            {
+                throw sdbusplus::xyz::openbmc_project::Common::Error::
+                    InvalidArgument();
+            }
+            cdw11 = (msg.aggregation_threshold() & 0xFF) |
+                    ((msg.aggregation_time() & 0xFF) << 8);
+            break;
+        }
+        case 0x09:
+        {
+            auto msg = parseProto<
+                google::gbmc::nvme::base::InterruptVectorConfiguration>(data);
+            if (msg.interrupt_vector() > 0xFFFF)
+            {
+                throw sdbusplus::xyz::openbmc_project::Common::Error::
+                    InvalidArgument();
+            }
+            cdw11 = (msg.interrupt_vector() & 0xFFFF) |
+                    ((static_cast<uint32_t>(msg.coalescing_disable()) & 0x1)
+                     << 16);
+            break;
+        }
+        case 0x0A:
+        {
+            auto msg =
+                parseProto<google::gbmc::nvme::base::WriteAtomicityNormal>(
+                    data);
+            cdw11 = static_cast<uint32_t>(msg.disable_normal()) & 0x1;
+            break;
+        }
+        case 0x0B:
+        {
+            auto msg = parseProto<
+                google::gbmc::nvme::base::AsynchronousEventConfiguration>(data);
+            if (msg.smart_critical_warning_config() > 0xFF)
+            {
+                throw sdbusplus::xyz::openbmc_project::Common::Error::
+                    InvalidArgument();
+            }
+            cdw11 = msg.smart_critical_warning_config() & 0xFF;
+            break;
+        }
+        default:
+            break;
+    }
+
+    NVMeMiIntf::SetFeaturesRequest args{};
+    args.fid = fid;
+    args.save = save;
+    args.cdw11 = cdw11;
+    args.nsid = (scope == FeatureScope::Namespace) ? nsid : NVME_NSID_NONE;
+    args.data = {};
+
+    auto [ex, resp] = asyncAdminSetFeatures(io, nvmeIntf, ctrl, args,
+                                            std::move(yield));
+
+    if (ex)
+    {
+        lg2::error("NVMe Set Features (FID {FID}) failed: {ERROR}", "FID", fid,
+                   "ERROR", ex->what());
+        throw sdbusplus::xyz::openbmc_project::Common::Error::Unavailable();
+    }
+}
diff --git a/src/NVMeFeatureStore.hpp b/src/NVMeFeatureStore.hpp
new file mode 100644
index 0000000..49832c7
--- /dev/null
+++ b/src/NVMeFeatureStore.hpp
@@ -0,0 +1,67 @@
+#pragma once
+
+#include "NVMeMi.hpp"
+
+#include <boost/asio/spawn.hpp>
+#include <sdbusplus/asio/object_server.hpp>
+
+#include <cstdint>
+#include <map>
+#include <memory>
+#include <string>
+#include <vector>
+
+enum class FeatureScope : std::uint8_t
+{
+    Controller,
+    Namespace,
+    Subsystem
+};
+
+class FeatureStore : public std::enable_shared_from_this<FeatureStore>
+{
+  public:
+    FeatureStore(boost::asio::io_context& io,
+                 sdbusplus::asio::object_server& objServer,
+                 const std::shared_ptr<sdbusplus::asio::connection>& conn,
+                 const std::string& objectPath,
+                 std::shared_ptr<NVMeMiIntf> nvmeIntf, nvme_mi_ctrl_t ctrl,
+                 uint32_t nsid, FeatureScope scope);
+
+    virtual ~FeatureStore() = default;
+
+    // Initialize the feature collection by querying the device
+    void init(std::function<void(const std::error_code&)>&& cb);
+
+  private:
+    boost::asio::io_context& io;
+    sdbusplus::asio::object_server& objServer;
+    std::shared_ptr<sdbusplus::asio::connection> conn;
+    std::string objectPath;
+    std::shared_ptr<NVMeMiIntf> nvmeIntf;
+    nvme_mi_ctrl_t ctrl;
+    uint32_t nsid;
+    FeatureScope scope;
+
+    bool vwcSupported = false;
+    uint16_t oncs = 0;
+
+    // Map of FID to its supported capabilities (from SEL=011b)
+    std::map<uint8_t, uint32_t> featureCaps;
+
+    std::shared_ptr<sdbusplus::asio::dbus_interface> dbusIntf;
+    std::vector<std::tuple<std::string, std::string, std::string>>
+        featureCollection;
+
+    // Helper to determine feature collection
+    void discoverFeatures(const boost::asio::yield_context& yield);
+    void fetchIdentifyData(const boost::asio::yield_context& yield);
+
+    void registerDbusInterface();
+
+    std::vector<uint8_t> getFeature(boost::asio::yield_context yield,
+                                    const std::string& featureName);
+    void setFeature(boost::asio::yield_context yield,
+                    const std::string& featureName,
+                    const std::vector<uint8_t>& data);
+};
diff --git a/src/meson.build b/src/meson.build
index ecbb881..9c0b69d 100644
--- a/src/meson.build
+++ b/src/meson.build
@@ -217,6 +217,7 @@
         'NVMeStorage.cpp',
         'NVMePlugin.cpp',
         'NVMeMetricStore.cpp',
+        'NVMeFeatureStore.cpp',
     )
 
     pdi_dep = phosphor_dbus_interfaces_dep
@@ -234,7 +235,7 @@
     nvme_deps = [ 
         default_deps, i2c, thresholds_dep, utils_dep, 
         threads, pdi_dep, libnvme, libnvme_mi,
-        nvme_dbus_dep
+        nvme_dbus_dep, nvme_proto_dep
     ]
 
     if get_option('nvme_shmem').enabled()
@@ -267,7 +268,7 @@
 
     nvme_plugin_dep = declare_dependency(
         include_directories: nvme_plugin_inc,
-        dependencies: [nvme_dbus_dep, pdi_dep, libnvme_mi], 
+        dependencies: [default_deps, nvme_dbus_dep, pdi_dep, libnvme_mi], 
     )
 
     pkgconfig = import('pkgconfig')
diff --git a/tests/test_nvme_mi.cpp b/tests/test_nvme_mi.cpp
index 5f1f02b..240b4cf 100644
--- a/tests/test_nvme_mi.cpp
+++ b/tests/test_nvme_mi.cpp
@@ -210,6 +210,7 @@
 {
   protected:
     NVMeTest() :
+        systemBus(std::make_shared<sdbusplus::asio::connection>(io)),
         object_server(systemBus),
         nvme_intf(NVMeIntf::create<::testing::NiceMock<NVMeMiMock>>(
             io, subsysPollTime / 10)),
@@ -219,15 +220,13 @@
                                                subsysPath, "NVMe_1",
                                                SensorData{}, nvme_intf, false))
     {
+        systemBus->request_name("xyz.openbmc_project.NVMeTest");
         subsys->unavailableMaxCount = 1;
         subsys->pollingInterval = subsysPollTime;
     }
 
     static void SetUpTestSuite()
     {
-        systemBus = std::make_shared<sdbusplus::asio::connection>(NVMeTest::io);
-        systemBus->request_name("xyz.openbmc_project.NVMeTest");
-
         // Load plugin shared libraries
         try
         {
@@ -260,14 +259,19 @@
 
     void TearDown() override
     {
+        if (subsys)
+        {
+            subsys->stop();
+        }
+        io.poll();
         io.restart();
     }
 
     static constexpr char subsysPath[] = // NOLINT
         "/xyz/openbmc_project/inventory/Test_Chassis/Test_NVMe";
 
-    static boost::asio::io_context io;
-    static std::shared_ptr<sdbusplus::asio::connection> systemBus;
+    boost::asio::io_context io;
+    std::shared_ptr<sdbusplus::asio::connection> systemBus;
     sdbusplus::asio::object_server object_server;
 
     NVMeIntf nvme_intf;
@@ -282,9 +286,6 @@
                                        : std::chrono::milliseconds(100);
 }();
 
-boost::asio::io_context NVMeTest::io;
-std::shared_ptr<sdbusplus::asio::connection> NVMeTest::systemBus;
-
 /**
  * @brief Test start and stop function of NVMeSubsystem
  *
diff --git a/yaml/xyz/openbmc_project/NVMe/FeatureStore.interface.yaml b/yaml/xyz/openbmc_project/NVMe/FeatureStore.interface.yaml
index de77d22..217fafe 100644
--- a/yaml/xyz/openbmc_project/NVMe/FeatureStore.interface.yaml
+++ b/yaml/xyz/openbmc_project/NVMe/FeatureStore.interface.yaml
@@ -57,3 +57,4 @@
       errors:
           - xyz.openbmc_project.Common.Error.InvalidArgument
           - xyz.openbmc_project.Common.Error.Unavailable
+          - xyz.openbmc_project.Common.Error.NotAllowed