diff --git a/src/oemcommands.cpp b/src/oemcommands.cpp
new file mode 100644
index 0000000..f30481a
--- /dev/null
+++ b/src/oemcommands.cpp
@@ -0,0 +1,562 @@
+/*
+ * Copyright (c)  2018 Intel Corporation.
+ * Copyright (c)  2018-present Facebook.
+ *
+ * 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 "xyz/openbmc_project/Common/error.hpp"
+#include <ipmid/api.h>
+
+#include <array>
+#include <commandutils.hpp>
+#include <cstring>
+#include <iostream>
+#include <oemcommands.hpp>
+#include <phosphor-ipmi-host/utils.hpp>
+#include <phosphor-logging/log.hpp>
+#include <sdbusplus/bus.hpp>
+#include <string>
+#include <vector>
+
+#define SIZE_IANA_ID 3
+
+namespace ipmi
+{
+static void registerOEMFunctions() __attribute__((constructor));
+sdbusplus::bus::bus dbus(ipmid_get_sd_bus_connection()); // from ipmid/api.h
+static constexpr size_t maxFRUStringLength = 0x3F;
+
+ipmi_ret_t plat_udbg_get_post_desc(uint8_t, uint8_t *, uint8_t, uint8_t *,
+                                   uint8_t *, uint8_t *);
+ipmi_ret_t plat_udbg_get_frame_data(uint8_t, uint8_t, uint8_t *, uint8_t *,
+                                    uint8_t *);
+ipmi_ret_t plat_udbg_control_panel(uint8_t, uint8_t, uint8_t, uint8_t *,
+                                   uint8_t *);
+
+// return code: 0 successful
+int8_t getFruData(std::string &data, std::string &name)
+{
+    std::string objpath = "/xyz/openbmc_project/FruDevice";
+    std::string intf = "xyz.openbmc_project.FruDeviceManager";
+    std::string service = getService(dbus, intf, objpath);
+    ObjectValueTree valueTree = getManagedObjects(dbus, service, "/");
+    if (valueTree.empty())
+    {
+        phosphor::logging::log<phosphor::logging::level::ERR>(
+            "No object implements interface",
+            phosphor::logging::entry("INTF=%s", intf.c_str()));
+        return -1;
+    }
+
+    for (const auto &item : valueTree)
+    {
+        auto interface = item.second.find("xyz.openbmc_project.FruDevice");
+        if (interface == item.second.end())
+        {
+            continue;
+        }
+
+        auto property = interface->second.find(name.c_str());
+        if (property == interface->second.end())
+        {
+            continue;
+        }
+
+        try
+        {
+            Value variant = property->second;
+            std::string &result =
+                sdbusplus::message::variant_ns::get<std::string>(variant);
+            if (result.size() > maxFRUStringLength)
+            {
+                phosphor::logging::log<phosphor::logging::level::ERR>(
+                    "FRU serial number exceed maximum length");
+                return -1;
+            }
+            data = result;
+            return 0;
+        }
+        catch (sdbusplus::message::variant_ns::bad_variant_access &e)
+        {
+            phosphor::logging::log<phosphor::logging::level::ERR>(e.what());
+            return -1;
+        }
+    }
+    return -1;
+}
+
+typedef struct
+{
+    uint8_t cur_power_state;
+    uint8_t last_power_event;
+    uint8_t misc_power_state;
+    uint8_t front_panel_button_cap_status;
+} ipmi_get_chassis_status_t;
+
+// Todo: Needs to update this as per power policy when integrated
+//----------------------------------------------------------------------
+// Get Chassis Status commands
+//----------------------------------------------------------------------
+ipmi_ret_t ipmiGetChassisStatus(ipmi_netfn_t netfn, ipmi_cmd_t cmd,
+                                ipmi_request_t request,
+                                ipmi_response_t response,
+                                ipmi_data_len_t data_len,
+                                ipmi_context_t context)
+{
+    ipmi_get_chassis_status_t chassis_status;
+    uint8_t s = 2;
+
+    *data_len = 4;
+
+    // Current Power State
+    // [7] reserved
+    // [6..5] power restore policy
+    //          00b = chassis stays powered off after AC/mains returns
+    //          01b = after AC returns, power is restored to the state that was
+    //          in effect when AC/mains was lost.
+    //          10b = chassis always powers up after AC/mains returns
+    //          11b = unknow
+    //        Set to 00b, by observing the hardware behavior.
+    //        Do we need to define a dbus property to identify the restore
+    //        policy?
+
+    // [4] power control fault
+    //       1b = controller attempted to turn system power on or off, but
+    //       system did not enter desired state.
+    //       Set to 0b, since We don't support it..
+
+    // [3] power fault
+    //       1b = fault detected in main power subsystem.
+    //       set to 0b. for we don't support it.
+
+    // [2] 1b = interlock (chassis is presently shut down because a chassis
+    //       panel interlock switch is active). (IPMI 1.5)
+    //       set to 0b,  for we don't support it.
+
+    // [1] power overload
+    //      1b = system shutdown because of power overload condition.
+    //       set to 0b,  for we don't support it.
+
+    // [0] power is on
+    //       1b = system power is on
+    //       0b = system power is off(soft-off S4/S5, or mechanical off)
+
+    chassis_status.cur_power_state = ((s & 0x3) << 5) | (1 & 0x1);
+
+    // Last Power Event
+    // [7..5] – reserved
+    // [4] – 1b = last ‘Power is on’ state was entered via IPMI command
+    // [3] – 1b = last power down caused by power fault
+    // [2] – 1b = last power down caused by a power interlock being activated
+    // [1] – 1b = last power down caused by a Power overload
+    // [0] – 1b = AC failed
+    // set to 0x0,  for we don't support these fields.
+
+    chassis_status.last_power_event = 0;
+
+    // Misc. Chassis State
+    // [7] – reserved
+    // [6] – 1b = Chassis Identify command and state info supported (Optional)
+    //       0b = Chassis Identify command support unspecified via this command.
+    //       (The Get Command Support command , if implemented, would still
+    //       indicate support for the Chassis Identify command)
+    // [5..4] – Chassis Identify State. Mandatory when bit[6] =1b, reserved
+    // (return
+    //          as 00b) otherwise. Returns the present chassis identify state.
+    //           Refer to the Chassis Identify command for more info.
+    //         00b = chassis identify state = Off
+    //         01b = chassis identify state = Temporary(timed) On
+    //         10b = chassis identify state = Indefinite On
+    //         11b = reserved
+    // [3] – 1b = Cooling/fan fault detected
+    // [2] – 1b = Drive Fault
+    // [1] – 1b = Front Panel Lockout active (power off and reset via chassis
+    //       push-buttons disabled.)
+    // [0] – 1b = Chassis Intrusion active
+    //  set to 0,  for we don't support them.
+    chassis_status.misc_power_state = 0x40;
+
+    //  Front Panel Button Capabilities and disable/enable status(Optional)
+    //  set to 0,  for we don't support them.
+    chassis_status.front_panel_button_cap_status = 0;
+
+    // Pack the actual response
+    std::memcpy(response, &chassis_status, *data_len);
+
+    return IPMI_CC_OK;
+}
+
+//----------------------------------------------------------------------
+// Get Debug Frame Info
+//----------------------------------------------------------------------
+ipmi_ret_t ipmiOemDbgGetFrameInfo(ipmi_netfn_t netfn, ipmi_cmd_t cmd,
+                                  ipmi_request_t request,
+                                  ipmi_response_t response,
+                                  ipmi_data_len_t data_len,
+                                  ipmi_context_t context)
+{
+    uint8_t *req = reinterpret_cast<uint8_t *>(request);
+    uint8_t *res = reinterpret_cast<uint8_t *>(response);
+    uint8_t num_frames = 3;
+
+    std::memcpy(res, req, SIZE_IANA_ID); // IANA ID
+    res[SIZE_IANA_ID] = num_frames;
+    *data_len = SIZE_IANA_ID + 1;
+
+    return IPMI_CC_OK;
+}
+
+//----------------------------------------------------------------------
+// Get Debug Updated Frames
+//----------------------------------------------------------------------
+ipmi_ret_t ipmiOemDbgGetUpdFrames(ipmi_netfn_t netfn, ipmi_cmd_t cmd,
+                                  ipmi_request_t request,
+                                  ipmi_response_t response,
+                                  ipmi_data_len_t data_len,
+                                  ipmi_context_t context)
+{
+    uint8_t *req = reinterpret_cast<uint8_t *>(request);
+    uint8_t *res = reinterpret_cast<uint8_t *>(response);
+    uint8_t num_updates = 3;
+    *data_len = 4;
+
+    std::memcpy(res, req, SIZE_IANA_ID); // IANA ID
+    res[SIZE_IANA_ID] = num_updates;
+    *data_len = SIZE_IANA_ID + num_updates + 1;
+    res[SIZE_IANA_ID + 1] = 1; // info page update
+    res[SIZE_IANA_ID + 2] = 2; // cri sel update
+    res[SIZE_IANA_ID + 3] = 3; // cri sensor update
+
+    return IPMI_CC_OK;
+}
+
+//----------------------------------------------------------------------
+// Get Debug POST Description
+//----------------------------------------------------------------------
+ipmi_ret_t ipmiOemDbgGetPostDesc(ipmi_netfn_t netfn, ipmi_cmd_t cmd,
+                                 ipmi_request_t request,
+                                 ipmi_response_t response,
+                                 ipmi_data_len_t data_len,
+                                 ipmi_context_t context)
+{
+    uint8_t *req = reinterpret_cast<uint8_t *>(request);
+    uint8_t *res = reinterpret_cast<uint8_t *>(response);
+    uint8_t index = 0;
+    uint8_t next = 0;
+    uint8_t end = 0;
+    uint8_t phase = 0;
+    uint8_t count = 0;
+    int ret;
+
+    index = req[3];
+    phase = req[4];
+
+    phosphor::logging::log<phosphor::logging::level::INFO>(
+        "Get POST Description Event");
+
+    ret = plat_udbg_get_post_desc(index, &next, phase, &end, &count, &res[8]);
+    if (ret)
+    {
+        memcpy(res, req, SIZE_IANA_ID); // IANA ID
+        *data_len = SIZE_IANA_ID;
+        return IPMI_CC_UNSPECIFIED_ERROR;
+    }
+
+    memcpy(res, req, SIZE_IANA_ID); // IANA ID
+    res[3] = index;
+    res[4] = next;
+    res[5] = phase;
+    res[6] = end;
+    res[7] = count;
+    *data_len = SIZE_IANA_ID + 5 + count;
+
+    return IPMI_CC_OK;
+}
+
+//----------------------------------------------------------------------
+// Get Debug GPIO Description
+//----------------------------------------------------------------------
+ipmi_ret_t ipmiOemDbgGetGpioDesc(ipmi_netfn_t netfn, ipmi_cmd_t cmd,
+                                 ipmi_request_t request,
+                                 ipmi_response_t response,
+                                 ipmi_data_len_t data_len,
+                                 ipmi_context_t context)
+{
+    uint8_t *req = reinterpret_cast<uint8_t *>(request);
+    uint8_t *res = reinterpret_cast<uint8_t *>(response);
+
+    phosphor::logging::log<phosphor::logging::level::INFO>(
+        "Get GPIO Description Event");
+
+    std::memcpy(res, req, SIZE_IANA_ID + 1); // IANA ID
+    *data_len = SIZE_IANA_ID + 1;
+
+    return IPMI_CC_OK;
+}
+
+//----------------------------------------------------------------------
+// Get Debug Frame Data
+//----------------------------------------------------------------------
+ipmi_ret_t ipmiOemDbgGetFrameData(ipmi_netfn_t netfn, ipmi_cmd_t cmd,
+                                  ipmi_request_t request,
+                                  ipmi_response_t response,
+                                  ipmi_data_len_t data_len,
+                                  ipmi_context_t context)
+{
+    uint8_t *req = reinterpret_cast<uint8_t *>(request);
+    uint8_t *res = reinterpret_cast<uint8_t *>(response);
+    uint8_t frame;
+    uint8_t page;
+    uint8_t next;
+    uint8_t count;
+    int ret;
+
+    frame = req[3];
+    page = req[4];
+    int fr = frame;
+    int pg = page;
+
+    ret = plat_udbg_get_frame_data(frame, page, &next, &count, &res[7]);
+    if (ret)
+    {
+        memcpy(res, req, SIZE_IANA_ID); // IANA ID
+        *data_len = SIZE_IANA_ID;
+        return IPMI_CC_UNSPECIFIED_ERROR;
+    }
+
+    memcpy(res, req, SIZE_IANA_ID); // IANA ID
+    res[3] = frame;
+    res[4] = page;
+    res[5] = next;
+    res[6] = count;
+    *data_len = SIZE_IANA_ID + 4 + count;
+
+    return IPMI_CC_OK;
+}
+
+//----------------------------------------------------------------------
+// Get Debug Control Panel
+//----------------------------------------------------------------------
+ipmi_ret_t ipmiOemDbgGetCtrlPanel(ipmi_netfn_t netfn, ipmi_cmd_t cmd,
+                                  ipmi_request_t request,
+                                  ipmi_response_t response,
+                                  ipmi_data_len_t data_len,
+                                  ipmi_context_t context)
+{
+    uint8_t *req = reinterpret_cast<uint8_t *>(request);
+    uint8_t *res = reinterpret_cast<uint8_t *>(response);
+
+    uint8_t panel;
+    uint8_t operation;
+    uint8_t item;
+    uint8_t count;
+    ipmi_ret_t ret;
+
+    panel = req[3];
+    operation = req[4];
+    item = req[5];
+
+    ret = plat_udbg_control_panel(panel, operation, item, &count, &res[3]);
+
+    std::memcpy(res, req, SIZE_IANA_ID); // IANA ID
+    *data_len = SIZE_IANA_ID + count;
+
+    return ret;
+}
+
+// Todo: Need to implement all below functions for oem commands
+//----------------------------------------------------------------------
+// Set Dimm Info (CMD_OEM_SET_DIMM_INFO)
+//----------------------------------------------------------------------
+ipmi_ret_t ipmiOemSetDimmInfo(ipmi_netfn_t netfn, ipmi_cmd_t cmd,
+                              ipmi_request_t request, ipmi_response_t response,
+                              ipmi_data_len_t data_len, ipmi_context_t context)
+{
+    uint8_t *req = reinterpret_cast<uint8_t *>(request);
+    uint8_t *res = reinterpret_cast<uint8_t *>(response);
+
+    std::memcpy(res, req, SIZE_IANA_ID + 1); // IANA ID
+    *data_len = SIZE_IANA_ID + 1;
+    *data_len = 0;
+
+    return IPMI_CC_OK;
+}
+
+//----------------------------------------------------------------------
+// Get Boot Order (CMD_OEM_GET_BOOT_ORDER)
+//----------------------------------------------------------------------
+ipmi_ret_t ipmiOemGetBootOrder(ipmi_netfn_t netfn, ipmi_cmd_t cmd,
+                               ipmi_request_t request, ipmi_response_t response,
+                               ipmi_data_len_t data_len, ipmi_context_t context)
+{
+    uint8_t *req = reinterpret_cast<uint8_t *>(request);
+    uint8_t *res = reinterpret_cast<uint8_t *>(response);
+
+    *res++ = 0x01;
+    *res++ = 0x00;
+    *res++ = 0x09;
+    *res++ = 0x02;
+    *res++ = 0x03;
+    *res++ = 0xff;
+    *data_len = 6;
+
+    return IPMI_CC_OK;
+}
+
+//----------------------------------------------------------------------
+// Set Machine Config Info (CMD_OEM_SET_MACHINE_CONFIG_INFO)
+//----------------------------------------------------------------------
+ipmi_ret_t ipmiOemSetMachineCfgInfo(ipmi_netfn_t netfn, ipmi_cmd_t cmd,
+                                    ipmi_request_t request,
+                                    ipmi_response_t response,
+                                    ipmi_data_len_t data_len,
+                                    ipmi_context_t context)
+{
+    uint8_t *req = reinterpret_cast<uint8_t *>(request);
+    uint8_t *res = reinterpret_cast<uint8_t *>(response);
+
+    *data_len = 0;
+
+    return IPMI_CC_OK;
+}
+
+//----------------------------------------------------------------------
+// Set POST start (CMD_OEM_SET_POST_START)
+//----------------------------------------------------------------------
+ipmi_ret_t ipmiOemSetPostStart(ipmi_netfn_t netfn, ipmi_cmd_t cmd,
+                               ipmi_request_t request, ipmi_response_t response,
+                               ipmi_data_len_t data_len, ipmi_context_t context)
+{
+    uint8_t *req = reinterpret_cast<uint8_t *>(request);
+    uint8_t *res = reinterpret_cast<uint8_t *>(response);
+
+    phosphor::logging::log<phosphor::logging::level::INFO>("POST Start Event");
+
+    *data_len = 0;
+    return IPMI_CC_OK;
+}
+
+//----------------------------------------------------------------------
+// Set POST End (CMD_OEM_SET_POST_END)
+//----------------------------------------------------------------------
+ipmi_ret_t ipmiOemSetPostEnd(ipmi_netfn_t netfn, ipmi_cmd_t cmd,
+                             ipmi_request_t request, ipmi_response_t response,
+                             ipmi_data_len_t data_len, ipmi_context_t context)
+{
+    uint8_t *req = reinterpret_cast<uint8_t *>(request);
+    uint8_t *res = reinterpret_cast<uint8_t *>(response);
+
+    phosphor::logging::log<phosphor::logging::level::INFO>("POST End Event");
+
+    *data_len = 0;
+    return IPMI_CC_OK;
+}
+
+//----------------------------------------------------------------------
+// Set Bios Flash Info (CMD_OEM_SET_BIOS_FLASH_INFO)
+//----------------------------------------------------------------------
+ipmi_ret_t ipmiOemSetBiosFlashInfo(ipmi_netfn_t netfn, ipmi_cmd_t cmd,
+                                   ipmi_request_t request,
+                                   ipmi_response_t response,
+                                   ipmi_data_len_t data_len,
+                                   ipmi_context_t context)
+{
+    uint8_t *req = reinterpret_cast<uint8_t *>(request);
+    uint8_t *res = reinterpret_cast<uint8_t *>(response);
+
+    *data_len = 0;
+    return IPMI_CC_OK;
+}
+
+//----------------------------------------------------------------------
+// Set PPR (CMD_OEM_SET_PPR)
+//----------------------------------------------------------------------
+ipmi_ret_t ipmiOemSetPpr(ipmi_netfn_t netfn, ipmi_cmd_t cmd,
+                         ipmi_request_t request, ipmi_response_t response,
+                         ipmi_data_len_t data_len, ipmi_context_t context)
+{
+    uint8_t *req = reinterpret_cast<uint8_t *>(request);
+    uint8_t *res = reinterpret_cast<uint8_t *>(response);
+
+    *data_len = 0;
+    return IPMI_CC_OK;
+}
+
+//----------------------------------------------------------------------
+// Get PPR (CMD_OEM_GET_PPR)
+//----------------------------------------------------------------------
+ipmi_ret_t ipmiOemGetPpr(ipmi_netfn_t netfn, ipmi_cmd_t cmd,
+                         ipmi_request_t request, ipmi_response_t response,
+                         ipmi_data_len_t data_len, ipmi_context_t context)
+{
+    uint8_t *req = reinterpret_cast<uint8_t *>(request);
+    uint8_t *res = reinterpret_cast<uint8_t *>(response);
+
+    res[0] = 0x00;
+    *data_len = 1;
+
+    return IPMI_CC_OK;
+}
+
+static void registerOEMFunctions(void)
+{
+    phosphor::logging::log<phosphor::logging::level::INFO>(
+        "Registering OEM commands");
+    ipmiPrintAndRegister(NETFUN_CHASSIS, 1, NULL, ipmiGetChassisStatus,
+                         PRIVILEGE_USER); // get chassis status
+    ipmiPrintAndRegister(NETFN_OEM_USB_DBG_REQ, CMD_OEM_USB_DBG_GET_FRAME_INFO,
+                         NULL, ipmiOemDbgGetFrameInfo,
+                         PRIVILEGE_USER); // get debug frame info
+    ipmiPrintAndRegister(NETFN_OEM_USB_DBG_REQ,
+                         CMD_OEM_USB_DBG_GET_UPDATED_FRAMES, NULL,
+                         ipmiOemDbgGetUpdFrames,
+                         PRIVILEGE_USER); // get debug updated frames
+    ipmiPrintAndRegister(NETFN_OEM_USB_DBG_REQ, CMD_OEM_USB_DBG_GET_POST_DESC,
+                         NULL, ipmiOemDbgGetPostDesc,
+                         PRIVILEGE_USER); // get debug post description
+    ipmiPrintAndRegister(NETFN_OEM_USB_DBG_REQ, CMD_OEM_USB_DBG_GET_GPIO_DESC,
+                         NULL, ipmiOemDbgGetGpioDesc,
+                         PRIVILEGE_USER); // get debug gpio description
+    ipmiPrintAndRegister(NETFN_OEM_USB_DBG_REQ, CMD_OEM_USB_DBG_GET_FRAME_DATA,
+                         NULL, ipmiOemDbgGetFrameData,
+                         PRIVILEGE_USER); // get debug frame data
+    ipmiPrintAndRegister(NETFN_OEM_USB_DBG_REQ, CMD_OEM_USB_DBG_CTRL_PANEL,
+                         NULL, ipmiOemDbgGetCtrlPanel,
+                         PRIVILEGE_USER); // get debug control panel
+    ipmiPrintAndRegister(NETFUN_NONE, CMD_OEM_SET_DIMM_INFO, NULL,
+                         ipmiOemSetDimmInfo,
+                         PRIVILEGE_USER); // Set Dimm Info
+    ipmiPrintAndRegister(NETFUN_NONE, CMD_OEM_GET_BOOT_ORDER, NULL,
+                         ipmiOemGetBootOrder,
+                         PRIVILEGE_USER); // Get Boot Order
+    ipmiPrintAndRegister(NETFUN_NONE, CMD_OEM_SET_MACHINE_CONFIG_INFO, NULL,
+                         ipmiOemSetMachineCfgInfo,
+                         PRIVILEGE_USER); // Set Machine Config Info
+    ipmiPrintAndRegister(NETFUN_NONE, CMD_OEM_SET_POST_START, NULL,
+                         ipmiOemSetPostStart,
+                         PRIVILEGE_USER); // Set POST start
+    ipmiPrintAndRegister(NETFUN_NONE, CMD_OEM_SET_POST_END, NULL,
+                         ipmiOemSetPostEnd,
+                         PRIVILEGE_USER); // Set POST End
+    ipmiPrintAndRegister(NETFUN_NONE, CMD_OEM_SET_BIOS_FLASH_INFO, NULL,
+                         ipmiOemSetBiosFlashInfo,
+                         PRIVILEGE_USER); // Set Bios Flash Info
+    ipmiPrintAndRegister(NETFUN_NONE, CMD_OEM_SET_PPR, NULL, ipmiOemSetPpr,
+                         PRIVILEGE_USER); // Set PPR
+    ipmiPrintAndRegister(NETFUN_NONE, CMD_OEM_GET_PPR, NULL, ipmiOemGetPpr,
+                         PRIVILEGE_USER); // Get PPR
+    return;
+}
+
+} // namespace ipmi
diff --git a/src/storagecommands.cpp b/src/storagecommands.cpp
new file mode 100644
index 0000000..acb7fee
--- /dev/null
+++ b/src/storagecommands.cpp
@@ -0,0 +1,841 @@
+/*
+ * Copyright (c)  2018 Intel Corporation.
+ * Copyright (c)  2018-present Facebook.
+ *
+ * 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 <ipmid/api.h>
+
+#include <boost/container/flat_map.hpp>
+#include <commandutils.hpp>
+#include <iostream>
+#include <phosphor-ipmi-host/utils.hpp>
+#include <phosphor-logging/log.hpp>
+#include <sdbusplus/message/types.hpp>
+#include <sdbusplus/timer.hpp>
+#include <sensorutils.hpp>
+#include <storagecommands.hpp>
+
+namespace ipmi
+{
+
+namespace storage
+{
+void registerStorageFunctions() __attribute__((constructor));
+
+constexpr static const size_t maxMessageSize = 64;
+constexpr static const size_t maxFruSdrNameSize = 16;
+static constexpr int sensorMapUpdatePeriod = 2;
+using SensorMap = std::map<std::string, std::map<std::string, DbusVariant>>;
+namespace variant_ns = sdbusplus::message::variant_ns;
+
+using ManagedObjectSensor =
+    std::map<sdbusplus::message::object_path,
+             std::map<std::string, std::map<std::string, DbusVariant>>>;
+
+static uint16_t sdrReservationID;
+
+static boost::container::flat_map<std::string, ManagedObjectSensor> SensorCache;
+static SensorSubTree sensorTree;
+
+void registerSensorFunctions() __attribute__((constructor));
+using ManagedObjectType = boost::container::flat_map<
+    sdbusplus::message::object_path,
+    boost::container::flat_map<
+        std::string, boost::container::flat_map<std::string, DbusVariant>>>;
+using ManagedEntry = std::pair<
+    sdbusplus::message::object_path,
+    boost::container::flat_map<
+        std::string, boost::container::flat_map<std::string, DbusVariant>>>;
+
+constexpr static const char *fruDeviceServiceName =
+    "xyz.openbmc_project.FruDevice";
+constexpr static const size_t cacheTimeoutSeconds = 10;
+
+static std::vector<uint8_t> fruCache;
+static uint8_t cacheBus = 0xFF;
+static uint8_t cacheAddr = 0XFF;
+
+std::unique_ptr<phosphor::Timer> cacheTimer = nullptr;
+
+// we unfortunately have to build a map of hashes in case there is a
+// collision to verify our dev-id
+boost::container::flat_map<uint8_t, std::pair<uint8_t, uint8_t>> deviceHashes;
+
+static sdbusplus::bus::bus dbus(ipmid_get_sd_bus_connection());
+
+static bool getSensorMap(std::string sensorConnection, std::string sensorPath,
+                         SensorMap &sensorMap)
+{
+    static boost::container::flat_map<
+        std::string, std::chrono::time_point<std::chrono::steady_clock>>
+        updateTimeMap;
+
+    auto updateFind = updateTimeMap.find(sensorConnection);
+    auto lastUpdate = std::chrono::time_point<std::chrono::steady_clock>();
+    if (updateFind != updateTimeMap.end())
+    {
+        lastUpdate = updateFind->second;
+    }
+
+    auto now = std::chrono::steady_clock::now();
+
+    if (std::chrono::duration_cast<std::chrono::seconds>(now - lastUpdate)
+            .count() > sensorMapUpdatePeriod)
+    {
+        updateTimeMap[sensorConnection] = now;
+
+        auto managedObj = dbus.new_method_call(
+            sensorConnection.c_str(), "/", "org.freedesktop.DBus.ObjectManager",
+            "GetManagedObjects");
+
+        ManagedObjectSensor managedObjects;
+        try
+        {
+            auto reply = dbus.call(managedObj);
+            reply.read(managedObjects);
+        }
+        catch (sdbusplus::exception_t &)
+        {
+            phosphor::logging::log<phosphor::logging::level::ERR>(
+                "Error getting managed objects from connection",
+                phosphor::logging::entry("CONNECTION=%s",
+                                         sensorConnection.c_str()));
+            return false;
+        }
+
+        SensorCache[sensorConnection] = managedObjects;
+    }
+    auto connection = SensorCache.find(sensorConnection);
+    if (connection == SensorCache.end())
+    {
+        return false;
+    }
+    auto path = connection->second.find(sensorPath);
+    if (path == connection->second.end())
+    {
+        return false;
+    }
+    sensorMap = path->second;
+
+    return true;
+}
+
+bool writeFru()
+{
+    sdbusplus::message::message writeFru = dbus.new_method_call(
+        fruDeviceServiceName, "/xyz/openbmc_project/FruDevice",
+        "xyz.openbmc_project.FruDeviceManager", "WriteFru");
+    writeFru.append(cacheBus, cacheAddr, fruCache);
+    try
+    {
+        sdbusplus::message::message writeFruResp = dbus.call(writeFru);
+    }
+    catch (sdbusplus::exception_t &)
+    {
+        // todo: log sel?
+        phosphor::logging::log<phosphor::logging::level::ERR>(
+            "error writing fru");
+        return false;
+    }
+    return true;
+}
+
+void createTimer()
+{
+    if (cacheTimer == nullptr)
+    {
+        cacheTimer = std::make_unique<phosphor::Timer>(writeFru);
+    }
+}
+
+ipmi_ret_t replaceCacheFru(uint8_t devId)
+{
+    static uint8_t lastDevId = 0xFF;
+
+    bool timerRunning = (cacheTimer != nullptr) && !cacheTimer->isExpired();
+    if (lastDevId == devId && timerRunning)
+    {
+        return IPMI_CC_OK; // cache already up to date
+    }
+    // if timer is running, stop it and writeFru manually
+    else if (timerRunning)
+    {
+        cacheTimer->stop();
+        writeFru();
+    }
+
+    sdbusplus::message::message getObjects = dbus.new_method_call(
+        fruDeviceServiceName, "/", "org.freedesktop.DBus.ObjectManager",
+        "GetManagedObjects");
+    ManagedObjectType frus;
+    try
+    {
+        sdbusplus::message::message resp = dbus.call(getObjects);
+        resp.read(frus);
+    }
+    catch (sdbusplus::exception_t &)
+    {
+        phosphor::logging::log<phosphor::logging::level::ERR>(
+            "replaceCacheFru: error getting managed objects");
+        return IPMI_CC_RESPONSE_ERROR;
+    }
+
+    deviceHashes.clear();
+
+    // hash the object paths to create unique device id's. increment on
+    // collision
+    std::hash<std::string> hasher;
+    for (const auto &fru : frus)
+    {
+        auto fruIface = fru.second.find("xyz.openbmc_project.FruDevice");
+        if (fruIface == fru.second.end())
+        {
+            continue;
+        }
+
+        auto busFind = fruIface->second.find("BUS");
+        auto addrFind = fruIface->second.find("ADDRESS");
+        if (busFind == fruIface->second.end() ||
+            addrFind == fruIface->second.end())
+        {
+            phosphor::logging::log<phosphor::logging::level::INFO>(
+                "fru device missing Bus or Address",
+                phosphor::logging::entry("FRU=%s", fru.first.str.c_str()));
+            continue;
+        }
+
+        uint8_t fruBus =
+            sdbusplus::message::variant_ns::get<uint32_t>(busFind->second);
+        uint8_t fruAddr =
+            sdbusplus::message::variant_ns::get<uint32_t>(addrFind->second);
+
+        uint8_t fruHash = 0;
+        // Need to revise this strategy for dev id
+        /*
+        if (fruBus != 0 || fruAddr != 0)
+        {
+          fruHash = hasher(fru.first.str);
+          // can't be 0xFF based on spec, and 0 is reserved for baseboard
+          if (fruHash == 0 || fruHash == 0xFF)
+          {
+            fruHash = 1;
+          }
+        }
+        */
+        std::pair<uint8_t, uint8_t> newDev(fruBus, fruAddr);
+
+        bool emplacePassed = false;
+        while (!emplacePassed)
+        {
+            auto resp = deviceHashes.emplace(fruHash, newDev);
+            emplacePassed = resp.second;
+            if (!emplacePassed)
+            {
+                fruHash++;
+                // can't be 0xFF based on spec, and 0 is reserved for
+                // baseboard
+                if (fruHash == 0XFF)
+                {
+                    fruHash = 0x1;
+                }
+            }
+        }
+    }
+    auto deviceFind = deviceHashes.find(devId);
+    if (deviceFind == deviceHashes.end())
+    {
+        return IPMI_CC_SENSOR_INVALID;
+    }
+
+    fruCache.clear();
+    sdbusplus::message::message getRawFru = dbus.new_method_call(
+        fruDeviceServiceName, "/xyz/openbmc_project/FruDevice",
+        "xyz.openbmc_project.FruDeviceManager", "GetRawFru");
+    cacheBus = deviceFind->second.first;
+    cacheAddr = deviceFind->second.second;
+    getRawFru.append(cacheBus, cacheAddr);
+    try
+    {
+        sdbusplus::message::message getRawResp = dbus.call(getRawFru);
+        getRawResp.read(fruCache);
+    }
+    catch (sdbusplus::exception_t &)
+    {
+        lastDevId = 0xFF;
+        cacheBus = 0xFF;
+        cacheAddr = 0xFF;
+        return IPMI_CC_RESPONSE_ERROR;
+    }
+
+    lastDevId = devId;
+    return IPMI_CC_OK;
+}
+
+ipmi_ret_t ipmiStorageReadFRUData(ipmi_netfn_t netfn, ipmi_cmd_t cmd,
+                                  ipmi_request_t request,
+                                  ipmi_response_t response,
+                                  ipmi_data_len_t dataLen,
+                                  ipmi_context_t context)
+{
+    if (*dataLen != 4)
+    {
+        *dataLen = 0;
+        return IPMI_CC_REQ_DATA_LEN_INVALID;
+    }
+    *dataLen = 0; // default to 0 in case of an error
+
+    auto req = static_cast<GetFRUAreaReq *>(request);
+
+    if (req->countToRead > maxMessageSize - 1)
+    {
+        return IPMI_CC_INVALID_FIELD_REQUEST;
+    }
+    ipmi_ret_t status = replaceCacheFru(req->fruDeviceID);
+
+    if (status != IPMI_CC_OK)
+    {
+        return status;
+    }
+
+    size_t fromFRUByteLen = 0;
+    if (req->countToRead + req->fruInventoryOffset < fruCache.size())
+    {
+        fromFRUByteLen = req->countToRead;
+    }
+    else if (fruCache.size() > req->fruInventoryOffset)
+    {
+        fromFRUByteLen = fruCache.size() - req->fruInventoryOffset;
+    }
+    size_t padByteLen = req->countToRead - fromFRUByteLen;
+    uint8_t *respPtr = static_cast<uint8_t *>(response);
+    *respPtr = req->countToRead;
+    std::copy(fruCache.begin() + req->fruInventoryOffset,
+              fruCache.begin() + req->fruInventoryOffset + fromFRUByteLen,
+              ++respPtr);
+    // if longer than the fru is requested, fill with 0xFF
+    if (padByteLen)
+    {
+        respPtr += fromFRUByteLen;
+        std::fill(respPtr, respPtr + padByteLen, 0xFF);
+    }
+    *dataLen = fromFRUByteLen + 1;
+
+    return IPMI_CC_OK;
+}
+
+ipmi_ret_t ipmiStorageWriteFRUData(ipmi_netfn_t netfn, ipmi_cmd_t cmd,
+                                   ipmi_request_t request,
+                                   ipmi_response_t response,
+                                   ipmi_data_len_t dataLen,
+                                   ipmi_context_t context)
+{
+    if (*dataLen < 4 ||
+        *dataLen >=
+            0xFF + 3) // count written return is one byte, so limit to one
+                      // byte of data after the three request data bytes
+    {
+        *dataLen = 0;
+        return IPMI_CC_REQ_DATA_LEN_INVALID;
+    }
+
+    auto req = static_cast<WriteFRUDataReq *>(request);
+    size_t writeLen = *dataLen - 3;
+    *dataLen = 0; // default to 0 in case of an error
+
+    ipmi_ret_t status = replaceCacheFru(req->fruDeviceID);
+    if (status != IPMI_CC_OK)
+    {
+        return status;
+    }
+    int lastWriteAddr = req->fruInventoryOffset + writeLen;
+    if (fruCache.size() < lastWriteAddr)
+    {
+        fruCache.resize(req->fruInventoryOffset + writeLen);
+    }
+
+    std::copy(req->data, req->data + writeLen,
+              fruCache.begin() + req->fruInventoryOffset);
+
+    bool atEnd = false;
+
+    if (fruCache.size() >= sizeof(FRUHeader))
+    {
+
+        FRUHeader *header = reinterpret_cast<FRUHeader *>(fruCache.data());
+
+        int lastRecordStart = std::max(
+            header->internalOffset,
+            std::max(header->chassisOffset,
+                     std::max(header->boardOffset, header->productOffset)));
+        // TODO: Handle Multi-Record FRUs?
+
+        lastRecordStart *= 8; // header starts in are multiples of 8 bytes
+
+        // get the length of the area in multiples of 8 bytes
+        if (lastWriteAddr > (lastRecordStart + 1))
+        {
+            // second byte in record area is the length
+            int areaLength(fruCache[lastRecordStart + 1]);
+            areaLength *= 8; // it is in multiples of 8 bytes
+
+            if (lastWriteAddr >= (areaLength + lastRecordStart))
+            {
+                atEnd = true;
+            }
+        }
+    }
+    uint8_t *respPtr = static_cast<uint8_t *>(response);
+    if (atEnd)
+    {
+        // cancel timer, we're at the end so might as well send it
+        cacheTimer->stop();
+        if (!writeFru())
+        {
+            return IPMI_CC_INVALID_FIELD_REQUEST;
+        }
+        *respPtr = std::min(fruCache.size(), static_cast<size_t>(0xFF));
+    }
+    else
+    {
+        // start a timer, if no further data is sent in cacheTimeoutSeconds
+        // seconds, check to see if it is valid
+        createTimer();
+        cacheTimer->start(std::chrono::duration_cast<std::chrono::microseconds>(
+            std::chrono::seconds(cacheTimeoutSeconds)));
+        *respPtr = 0;
+    }
+
+    *dataLen = 1;
+
+    return IPMI_CC_OK;
+}
+
+ipmi_ret_t getFruSdrCount(size_t &count)
+{
+    ipmi_ret_t ret = replaceCacheFru(0);
+    if (ret != IPMI_CC_OK)
+    {
+        return ret;
+    }
+    count = deviceHashes.size();
+    return IPMI_CC_OK;
+}
+
+ipmi_ret_t getFruSdrs(size_t index, get_sdr::SensorDataFruRecord &resp)
+{
+    ipmi_ret_t ret = replaceCacheFru(0); // this will update the hash list
+    if (ret != IPMI_CC_OK)
+    {
+        return ret;
+    }
+    if (deviceHashes.size() < index)
+    {
+        return IPMI_CC_INVALID_FIELD_REQUEST;
+    }
+    auto device = deviceHashes.begin() + index;
+    uint8_t &bus = device->second.first;
+    uint8_t &address = device->second.second;
+
+    ManagedObjectType frus;
+
+    sdbusplus::message::message getObjects = dbus.new_method_call(
+        fruDeviceServiceName, "/", "org.freedesktop.DBus.ObjectManager",
+        "GetManagedObjects");
+    try
+    {
+        sdbusplus::message::message resp = dbus.call(getObjects);
+        resp.read(frus);
+    }
+    catch (sdbusplus::exception_t &)
+    {
+        return IPMI_CC_RESPONSE_ERROR;
+    }
+    boost::container::flat_map<std::string, DbusVariant> *fruData = nullptr;
+    auto fru =
+        std::find_if(frus.begin(), frus.end(),
+                     [bus, address, &fruData](ManagedEntry &entry) {
+                         auto findFruDevice =
+                             entry.second.find("xyz.openbmc_project.FruDevice");
+                         if (findFruDevice == entry.second.end())
+                         {
+                             return false;
+                         }
+                         fruData = &(findFruDevice->second);
+                         auto findBus = findFruDevice->second.find("BUS");
+                         auto findAddress =
+                             findFruDevice->second.find("ADDRESS");
+                         if (findBus == findFruDevice->second.end() ||
+                             findAddress == findFruDevice->second.end())
+                         {
+                             return false;
+                         }
+                         if (sdbusplus::message::variant_ns::get<uint32_t>(
+                                 findBus->second) != bus)
+                         {
+                             return false;
+                         }
+                         if (sdbusplus::message::variant_ns::get<uint32_t>(
+                                 findAddress->second) != address)
+                         {
+                             return false;
+                         }
+                         return true;
+                     });
+    if (fru == frus.end())
+    {
+        return IPMI_CC_RESPONSE_ERROR;
+    }
+    std::string name;
+    auto findProductName = fruData->find("BOARD_PRODUCT_NAME");
+    auto findBoardName = fruData->find("PRODUCT_PRODUCT_NAME");
+    if (findProductName != fruData->end())
+    {
+        name = sdbusplus::message::variant_ns::get<std::string>(
+            findProductName->second);
+    }
+    else if (findBoardName != fruData->end())
+    {
+        name = sdbusplus::message::variant_ns::get<std::string>(
+            findBoardName->second);
+    }
+    else
+    {
+        name = "UNKNOWN";
+    }
+    if (name.size() > maxFruSdrNameSize)
+    {
+        name = name.substr(0, maxFruSdrNameSize);
+    }
+    size_t sizeDiff = maxFruSdrNameSize - name.size();
+
+    resp.header.record_id_lsb = 0x0; // calling code is to implement these
+    resp.header.record_id_msb = 0x0;
+    resp.header.sdr_version = ipmiSdrVersion;
+    resp.header.record_type = 0x11; // FRU Device Locator
+    resp.header.record_length = sizeof(resp.body) + sizeof(resp.key) - sizeDiff;
+    resp.key.deviceAddress = 0x20;
+    resp.key.fruID = device->first;
+    resp.key.accessLun = 0x80; // logical / physical fru device
+    resp.key.channelNumber = 0x0;
+    resp.body.reserved = 0x0;
+    resp.body.deviceType = 0x10;
+    resp.body.entityID = 0x0;
+    resp.body.entityInstance = 0x1;
+    resp.body.oem = 0x0;
+    resp.body.deviceIDLen = name.size();
+    name.copy(resp.body.deviceID, name.size());
+
+    return IPMI_CC_OK;
+}
+
+ipmi_ret_t ipmiStorageReserveSDR(ipmi_netfn_t netfn, ipmi_cmd_t cmd,
+                                 ipmi_request_t request,
+                                 ipmi_response_t response,
+                                 ipmi_data_len_t dataLen,
+                                 ipmi_context_t context)
+{
+    printCommand(+netfn, +cmd);
+
+    if (*dataLen)
+    {
+        *dataLen = 0;
+        return IPMI_CC_REQ_DATA_LEN_INVALID;
+    }
+    *dataLen = 0; // default to 0 in case of an error
+    sdrReservationID++;
+    if (sdrReservationID == 0)
+    {
+        sdrReservationID++;
+    }
+    *dataLen = 2;
+    auto resp = static_cast<uint8_t *>(response);
+    resp[0] = sdrReservationID & 0xFF;
+    resp[1] = sdrReservationID >> 8;
+
+    return IPMI_CC_OK;
+}
+
+ipmi_ret_t ipmiStorageGetSDR(ipmi_netfn_t netfn, ipmi_cmd_t cmd,
+                             ipmi_request_t request, ipmi_response_t response,
+                             ipmi_data_len_t dataLen, ipmi_context_t context)
+{
+    printCommand(+netfn, +cmd);
+
+    if (*dataLen != 6)
+    {
+        *dataLen = 0;
+        return IPMI_CC_REQ_DATA_LEN_INVALID;
+    }
+    auto requestedSize = *dataLen;
+    *dataLen = 0; // default to 0 in case of an error
+
+    constexpr uint16_t lastRecordIndex = 0xFFFF;
+    auto req = static_cast<GetSDRReq *>(request);
+
+    // reservation required for partial reads with non zero offset into
+    // record
+    if ((sdrReservationID == 0 || req->reservationID != sdrReservationID) &&
+        req->offset)
+    {
+        return IPMI_CC_INVALID_RESERVATION_ID;
+    }
+
+    if (sensorTree.empty() && !getSensorSubtree(sensorTree))
+    {
+        return IPMI_CC_RESPONSE_ERROR;
+    }
+
+    size_t fruCount = 0;
+    ipmi_ret_t ret = ipmi::storage::getFruSdrCount(fruCount);
+    if (ret != IPMI_CC_OK)
+    {
+        return ret;
+    }
+
+    size_t lastRecord = sensorTree.size() + fruCount - 1;
+    if (req->recordID == lastRecordIndex)
+    {
+        req->recordID = lastRecord;
+    }
+    if (req->recordID > lastRecord)
+    {
+        return IPMI_CC_INVALID_FIELD_REQUEST;
+    }
+
+    uint16_t nextRecord =
+        lastRecord > (req->recordID + 1) ? req->recordID + 1 : 0XFFFF;
+
+    auto responseClear = static_cast<uint8_t *>(response);
+    std::fill(responseClear, responseClear + requestedSize, 0);
+
+    auto resp = static_cast<get_sdr::GetSdrResp *>(response);
+    resp->next_record_id_lsb = nextRecord & 0xFF;
+    resp->next_record_id_msb = nextRecord >> 8;
+
+    if (req->recordID >= sensorTree.size())
+    {
+        size_t fruIndex = req->recordID - sensorTree.size();
+        if (fruIndex >= fruCount)
+        {
+            return IPMI_CC_INVALID_FIELD_REQUEST;
+        }
+        get_sdr::SensorDataFruRecord data;
+        if (req->offset > sizeof(data))
+        {
+            return IPMI_CC_INVALID_FIELD_REQUEST;
+        }
+        ret = ipmi::storage::getFruSdrs(fruIndex, data);
+        if (ret != IPMI_CC_OK)
+        {
+            return ret;
+        }
+        data.header.record_id_msb = req->recordID << 8;
+        data.header.record_id_lsb = req->recordID & 0xFF;
+        if (sizeof(data) < (req->offset + req->bytesToRead))
+        {
+            req->bytesToRead = sizeof(data) - req->offset;
+        }
+        *dataLen = req->bytesToRead + 2; // next record
+        std::memcpy(&resp->record_data, (char *)&data + req->offset,
+                    req->bytesToRead);
+        return IPMI_CC_OK;
+    }
+
+    std::string connection;
+    std::string path;
+    uint16_t sensorIndex = req->recordID;
+    for (const auto &sensor : sensorTree)
+    {
+        if (sensorIndex-- == 0)
+        {
+            if (!sensor.second.size())
+            {
+                return IPMI_CC_RESPONSE_ERROR;
+            }
+            connection = sensor.second.begin()->first;
+            path = sensor.first;
+            break;
+        }
+    }
+
+    SensorMap sensorMap;
+    if (!getSensorMap(connection, path, sensorMap))
+    {
+        return IPMI_CC_RESPONSE_ERROR;
+    }
+    uint8_t sensornumber = (req->recordID & 0xFF);
+    get_sdr::SensorDataFullRecord record = {0};
+
+    record.header.record_id_msb = req->recordID << 8;
+    record.header.record_id_lsb = req->recordID & 0xFF;
+    record.header.sdr_version = ipmiSdrVersion;
+    record.header.record_type = get_sdr::SENSOR_DATA_FULL_RECORD;
+    record.header.record_length = sizeof(get_sdr::SensorDataFullRecord) -
+                                  sizeof(get_sdr::SensorDataRecordHeader);
+    record.key.owner_id = 0x20;
+    record.key.owner_lun = 0x0;
+    record.key.sensor_number = sensornumber;
+
+    record.body.entity_id = 0x0;
+    record.body.entity_instance = 0x01;
+    record.body.sensor_capabilities = 0x60; // auto rearm - todo hysteresis
+    record.body.sensor_type = getSensorTypeFromPath(path);
+    std::string type = getSensorTypeStringFromPath(path);
+    auto typeCstr = type.c_str();
+    auto findUnits = sensorUnits.find(typeCstr);
+    if (findUnits != sensorUnits.end())
+    {
+        record.body.sensor_units_2_base =
+            static_cast<uint8_t>(findUnits->second);
+    } // else default 0x0 unspecified
+
+    record.body.event_reading_type = getSensorEventTypeFromPath(path);
+
+    auto sensorObject = sensorMap.find("xyz.openbmc_project.Sensor.Value");
+    if (sensorObject == sensorMap.end())
+    {
+        return IPMI_CC_RESPONSE_ERROR;
+    }
+
+    auto maxObject = sensorObject->second.find("MaxValue");
+    auto minObject = sensorObject->second.find("MinValue");
+    double max = 128;
+    double min = -127;
+    if (maxObject != sensorObject->second.end())
+    {
+        max = variant_ns::visit(VariantToDoubleVisitor(), maxObject->second);
+    }
+
+    if (minObject != sensorObject->second.end())
+    {
+        min = variant_ns::visit(VariantToDoubleVisitor(), minObject->second);
+    }
+
+    int16_t mValue;
+    int8_t rExp;
+    int16_t bValue;
+    int8_t bExp;
+    bool bSigned;
+
+    if (!getSensorAttributes(max, min, mValue, rExp, bValue, bExp, bSigned))
+    {
+        return IPMI_CC_RESPONSE_ERROR;
+    }
+
+    // apply M, B, and exponents, M and B are 10 bit values, exponents are 4
+    record.body.m_lsb = mValue & 0xFF;
+
+    // move the smallest bit of the MSB into place (bit 9)
+    // the MSbs are bits 7:8 in m_msb_and_tolerance
+    uint8_t mMsb = (mValue & (1 << 8)) > 0 ? (1 << 6) : 0;
+
+    // assign the negative
+    if (mValue < 0)
+    {
+        mMsb |= (1 << 7);
+    }
+    record.body.m_msb_and_tolerance = mMsb;
+
+    record.body.b_lsb = bValue & 0xFF;
+
+    // move the smallest bit of the MSB into place
+    // the MSbs are bits 7:8 in b_msb_and_accuracy_lsb
+    uint8_t bMsb = (bValue & (1 << 8)) > 0 ? (1 << 6) : 0;
+
+    // assign the negative
+    if (bValue < 0)
+    {
+        bMsb |= (1 << 7);
+    }
+    record.body.b_msb_and_accuracy_lsb = bMsb;
+
+    record.body.r_b_exponents = bExp & 0x7;
+    if (bExp < 0)
+    {
+        record.body.r_b_exponents |= 1 << 3;
+    }
+    record.body.r_b_exponents = (rExp & 0x7) << 4;
+    if (rExp < 0)
+    {
+        record.body.r_b_exponents |= 1 << 7;
+    }
+
+    // todo fill out rest of units
+    if (bSigned)
+    {
+        record.body.sensor_units_1 = 1 << 7;
+    }
+
+    // populate sensor name from path
+    std::string name;
+    size_t nameStart = path.rfind("/");
+    if (nameStart != std::string::npos)
+    {
+        name = path.substr(nameStart + 1, std::string::npos - nameStart);
+    }
+
+    std::replace(name.begin(), name.end(), '_', ' ');
+    if (name.size() > FULL_RECORD_ID_STR_MAX_LENGTH)
+    {
+        name.resize(FULL_RECORD_ID_STR_MAX_LENGTH);
+    }
+    record.body.id_string_info = name.size();
+    std::strncpy(record.body.id_string, name.c_str(),
+                 sizeof(record.body.id_string));
+
+    if (sizeof(get_sdr::SensorDataFullRecord) <
+        (req->offset + req->bytesToRead))
+    {
+        req->bytesToRead = sizeof(get_sdr::SensorDataFullRecord) - req->offset;
+    }
+
+    *dataLen =
+        2 + req->bytesToRead; // bytesToRead + MSB and LSB of next record id
+
+    std::memcpy(&resp->record_data, (char *)&record + req->offset,
+                req->bytesToRead);
+
+    return IPMI_CC_OK;
+}
+
+void registerStorageFunctions()
+{
+    // <READ FRU Data>
+    ipmiPrintAndRegister(
+        NETFUN_STORAGE,
+        static_cast<ipmi_cmd_t>(IPMINetfnStorageCmds::ipmiCmdReadFRUData), NULL,
+        ipmiStorageReadFRUData, PRIVILEGE_OPERATOR);
+
+    // <WRITE FRU Data>
+    ipmiPrintAndRegister(
+        NETFUN_STORAGE,
+        static_cast<ipmi_cmd_t>(IPMINetfnStorageCmds::ipmiCmdWriteFRUData),
+        NULL, ipmiStorageWriteFRUData, PRIVILEGE_OPERATOR);
+
+    // <Reserve SDR Repo>
+    ipmiPrintAndRegister(
+        NETFUN_STORAGE,
+        static_cast<ipmi_cmd_t>(IPMINetfnStorageCmds::ipmiCmdReserveSDR),
+        nullptr, ipmiStorageReserveSDR, PRIVILEGE_USER);
+
+    // <Get Sdr>
+    ipmiPrintAndRegister(
+        NETFUN_STORAGE,
+        static_cast<ipmi_cmd_t>(IPMINetfnStorageCmds::ipmiCmdGetSDR), nullptr,
+        ipmiStorageGetSDR, PRIVILEGE_USER);
+    return;
+}
+} // namespace storage
+} // namespace ipmi
diff --git a/src/usb-dbg.cpp b/src/usb-dbg.cpp
new file mode 100644
index 0000000..efa0136
--- /dev/null
+++ b/src/usb-dbg.cpp
@@ -0,0 +1,952 @@
+/*
+ * Copyright (c)  2018-present Facebook. All Rights Reserved.
+ *
+ * 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 <host-ipmid/ipmid-api.h>
+#include <stdbool.h>
+#include <stdio.h>
+#include <sys/stat.h>
+
+#include <fstream>
+#include <iostream>
+
+namespace ipmi
+{
+
+#define ETH_INTF_NAME "eth0"
+
+#define ESCAPE "\x1B"
+#define ESC_BAT ESCAPE "B"
+#define ESC_MCU_BL_VER ESCAPE "U"
+#define ESC_MCU_RUN_VER ESCAPE "R"
+#define ESC_ALT ESCAPE "[5;7m"
+#define ESC_RST ESCAPE "[m"
+
+#define LINE_DELIMITER '\x1F'
+
+#define FRAME_BUFF_SIZE 4096
+#define FRAME_PAGE_BUF_SIZE 256
+#define FRU_ALL 0
+#define MAX_VALUE_LEN 64
+
+/* Used for systems which do not specifically have a
+ * phase, and we want to ignore the phase provided by the
+ * debug card */
+#define PHASE_ANY 0xff
+
+ipmi_ret_t getNetworkData(uint8_t lan_param, char *data);
+int8_t getFruData(std::string &serial, std::string &name);
+
+typedef struct _post_desc
+{
+    uint8_t code;
+    char desc[32];
+} post_desc_t;
+
+typedef struct _post_phase_desc
+{
+    int phase;
+    post_desc_t *post_tbl;
+    size_t post_tbl_cnt;
+} post_phase_desc_t;
+
+typedef struct _gpio_desc
+{
+    uint8_t pin;
+    uint8_t level;
+    uint8_t def;
+    char desc[32];
+} gpio_desc_t;
+
+typedef struct _sensor_desc
+{
+    char name[16];
+    uint8_t sensor_num;
+    char unit[5];
+    uint8_t fru;
+    uint8_t disp_prec;
+} sensor_desc_t;
+
+struct frame
+{
+    char title[32];
+    size_t max_size;
+    size_t max_page;
+    char *buf;
+    uint16_t idx_head, idx_tail;
+    uint8_t line_per_page;
+    uint8_t line_width;
+    uint16_t lines, pages;
+    uint8_t esc_sts;
+    uint8_t overwrite;
+    time_t mtime;
+    frame() : buf(NULL), pages(0), mtime(0)
+    {
+    }
+    int init(size_t size);
+    int append(const char *string, int indent);
+    int insert(const char *string, int indent);
+    int getPage(int page, char *page_buf, size_t page_buf_size);
+    int isFull();
+    int isEscSeq(char chr);
+    int parse(char *buf, size_t buf_size, const char *input, int indent);
+};
+
+struct frame frame_info;
+struct frame frame_sel;
+struct frame frame_snr;
+
+enum ENUM_PANEL
+{
+    PANEL_MAIN = 1,
+    PANEL_BOOT_ORDER = 2,
+    PANEL_POWER_POLICY = 3,
+};
+
+struct ctrl_panel
+{
+    uint8_t parent;
+    uint8_t item_num;
+    char item_str[8][32];
+    uint8_t (*select)(uint8_t item);
+};
+
+static uint8_t panel_main(uint8_t item);
+static uint8_t panel_boot_order(uint8_t item);
+static uint8_t panel_power_policy(uint8_t item);
+
+static struct ctrl_panel panels[] = {
+    {/* dummy entry for making other to 1-based */},
+    {
+        .parent = PANEL_MAIN,
+        .item_num = 2,
+        .item_str =
+            {
+                "User Setting",
+                ">Boot Order",
+                ">Power Policy",
+            },
+        .select = panel_main,
+    },
+    {
+        .parent = PANEL_MAIN,
+        .item_num = 0,
+        .item_str =
+            {
+                "Boot Order",
+            },
+        .select = panel_boot_order,
+    },
+    {
+        .parent = PANEL_MAIN,
+        .item_num = 0,
+        .item_str =
+            {
+                "Power Policy",
+            },
+        .select = panel_power_policy,
+    },
+};
+
+static int panelNum = (sizeof(panels) / sizeof(struct ctrl_panel)) - 1;
+
+/* Returns the FRU the hand-switch is switched to. If it is switched to BMC
+ * it returns FRU_ALL. Note, if in err, it returns FRU_ALL */
+static uint8_t plat_get_fru_sel()
+{
+    // For Tiogapass it just return 1, can modify to support more platform
+    return 1;
+}
+
+// return 0 on seccuess
+int frame::init(size_t size)
+{
+    // Reset status
+    idx_head = idx_tail = 0;
+    lines = 0;
+    esc_sts = 0;
+    pages = 1;
+
+    if (buf != NULL && max_size == size)
+    {
+        // reinit
+        return 0;
+    }
+
+    if (buf != NULL && max_size != size)
+    {
+        delete[] buf;
+    }
+    // Initialize Configuration
+    title[0] = '\0';
+    buf = new char[size];
+    max_size = size;
+    max_page = size;
+    line_per_page = 7;
+    line_width = 16;
+    overwrite = 0;
+
+    if (buf)
+        return 0;
+    else
+        return -1;
+}
+
+// return 0 on seccuess
+int frame::append(const char *string, int indent)
+{
+    const size_t buf_size = 64;
+    char lbuf[buf_size];
+    char *ptr;
+    int ret;
+
+    ret = parse(lbuf, buf_size, string, indent);
+
+    if (ret < 0)
+        return ret;
+
+    int len = strlen(string);
+    for (ptr = lbuf; *ptr != '\0'; ptr++)
+    {
+        if (isFull())
+        {
+            if (overwrite)
+            {
+                if (buf[idx_head] == LINE_DELIMITER)
+                    lines--;
+                idx_head = (idx_head + 1) % max_size;
+            }
+            else
+                return -1;
+        }
+
+        buf[idx_tail] = *ptr;
+        if (*ptr == LINE_DELIMITER)
+            lines++;
+
+        idx_tail = (idx_tail + 1) % max_size;
+    }
+
+    pages = (lines / line_per_page) + ((lines % line_per_page) ? 1 : 0);
+
+    if (pages > max_page)
+        pages = max_page;
+
+    return 0;
+}
+
+// return 0 on seccuess
+int frame::insert(const char *string, int indent)
+{
+    const size_t buf_size = 128;
+    char lbuf[buf_size];
+    char *ptr;
+    int ret;
+    int i;
+
+    ret = parse(lbuf, buf_size, string, indent);
+
+    if (ret < 0)
+        return ret;
+
+    for (i = strlen(lbuf) - 1; i >= 0; i--)
+    {
+        ptr = &lbuf[i];
+        if (isFull())
+        {
+            if (overwrite)
+            {
+                idx_tail = (idx_tail + max_size - 1) % max_size;
+                if (buf[idx_tail] == LINE_DELIMITER)
+                    lines--;
+            }
+            else
+                return -1;
+        }
+
+        idx_head = (idx_head + max_size - 1) % max_size;
+
+        buf[idx_head] = *ptr;
+        if (*ptr == LINE_DELIMITER)
+            lines++;
+    }
+
+    pages = (lines / line_per_page) + ((lines % line_per_page) ? 1 : 0);
+
+    if (pages > max_page)
+        pages = max_page;
+
+    return 0;
+}
+
+// return page size
+int frame::getPage(int page, char *page_buf, size_t page_buf_size)
+{
+    int ret;
+    uint16_t line = 0;
+    uint16_t idx, len;
+
+    if (buf == NULL)
+        return -1;
+
+    // 1-based page
+    if (page > pages || page < 1)
+        return -1;
+
+    if (page_buf == NULL || page_buf_size < 0)
+        return -1;
+
+    ret = snprintf(page_buf, 17, "%-10s %02d/%02d", title, page, pages);
+    len = strlen(page_buf);
+    if (ret < 0)
+        return -1;
+
+    line = 0;
+    idx = idx_head;
+    while (line < ((page - 1) * line_per_page) && idx != idx_tail)
+    {
+        if (buf[idx] == LINE_DELIMITER)
+            line++;
+        idx = (idx + 1) % max_size;
+    }
+
+    while (line < ((page)*line_per_page) && idx != idx_tail)
+    {
+        if (buf[idx] == LINE_DELIMITER)
+        {
+            line++;
+        }
+        else
+        {
+            page_buf[len++] = buf[idx];
+            if (len == (page_buf_size - 1))
+            {
+                break;
+            }
+        }
+        idx = (idx + 1) % max_size;
+    }
+
+    return len;
+}
+
+// return 1 for frame buffer full
+int frame::isFull()
+{
+    if (buf == NULL)
+        return -1;
+
+    if ((idx_tail + 1) % max_size == idx_head)
+        return 1;
+    else
+        return 0;
+}
+
+// return 1 for Escape Sequence
+int frame::isEscSeq(char chr)
+{
+    uint8_t curr_sts = esc_sts;
+
+    if (esc_sts == 0 && (chr == 0x1b))
+        esc_sts = 1; // Escape Sequence
+    else if (esc_sts == 1 && (chr == 0x5b))
+        esc_sts = 2; // Control Sequence Introducer(CSI)
+    else if (esc_sts == 1 && (chr != 0x5b))
+        esc_sts = 0;
+    else if (esc_sts == 2 && (chr >= 0x40 && chr <= 0x7e))
+        esc_sts = 0;
+
+    if (curr_sts || esc_sts)
+        return 1;
+    else
+        return 0;
+}
+
+// return 0 on success
+int frame::parse(char *lbuf, size_t buf_size, const char *input, int indent)
+{
+    uint8_t pos, esc;
+    int i;
+    const char *in, *end;
+
+    if (buf == NULL || input == NULL)
+        return -1;
+
+    if (indent >= line_width || indent < 0)
+        return -1;
+
+    in = input;
+    end = in + strlen(input);
+    pos = 0; // line position
+    esc = 0; // escape state
+    i = 0;   // buf index
+    while (in != end)
+    {
+        if (i >= buf_size)
+            break;
+
+        if (pos < indent)
+        {
+            // fill indent
+            lbuf[i++] = ' ';
+            pos++;
+            continue;
+        }
+
+        esc = isEscSeq(*in);
+
+        if (!esc && pos == line_width)
+        {
+            lbuf[i++] = LINE_DELIMITER;
+            pos = 0;
+            continue;
+        }
+
+        if (!esc)
+            pos++;
+
+        // fill input data
+        lbuf[i++] = *(in++);
+    }
+
+    // padding
+    while (pos <= line_width)
+    {
+        if (i >= buf_size)
+            break;
+        if (pos < line_width)
+            lbuf[i++] = ' ';
+        else
+            lbuf[i++] = LINE_DELIMITER;
+        pos++;
+    }
+
+    // full
+    if (i >= buf_size)
+        return -1;
+
+    lbuf[i++] = '\0';
+
+    return 0;
+}
+
+static int chk_cri_sel_update(uint8_t *cri_sel_up)
+{
+    FILE *fp;
+    struct stat file_stat;
+    uint8_t pos = plat_get_fru_sel();
+    static uint8_t pre_pos = 0xff;
+
+    fp = fopen("/mnt/data/cri_sel", "r");
+    if (fp)
+    {
+        if ((stat("/mnt/data/cri_sel", &file_stat) == 0) &&
+            (file_stat.st_mtime != frame_sel.mtime || pre_pos != pos))
+        {
+            *cri_sel_up = 1;
+        }
+        else
+        {
+            *cri_sel_up = 0;
+        }
+        fclose(fp);
+    }
+    else
+    {
+        if (frame_sel.buf == NULL || frame_sel.lines != 0 || pre_pos != pos)
+        {
+            *cri_sel_up = 1;
+        }
+        else
+        {
+            *cri_sel_up = 0;
+        }
+    }
+    pre_pos = pos;
+    return 0;
+}
+
+int plat_udbg_get_frame_info(uint8_t *num)
+{
+    *num = 3;
+    return 0;
+}
+
+int plat_udbg_get_updated_frames(uint8_t *count, uint8_t *buffer)
+{
+    uint8_t cri_sel_up = 0;
+    uint8_t info_page_up = 1;
+
+    *count = 0;
+
+    // info page update
+    if (info_page_up == 1)
+    {
+        buffer[*count] = 1;
+        *count += 1;
+    }
+
+    // cri sel update
+    chk_cri_sel_update(&cri_sel_up);
+    if (cri_sel_up == 1)
+    {
+        buffer[*count] = 2;
+        *count += 1;
+    }
+
+    // cri sensor update
+    buffer[*count] = 3;
+    *count += 1;
+
+    return 0;
+}
+
+int plat_udbg_get_post_desc(uint8_t index, uint8_t *next, uint8_t phase,
+                            uint8_t *end, uint8_t *length, uint8_t *buffer)
+{
+    int target, pdesc_size;
+    post_phase_desc_t *post_phase;
+    size_t post_phase_desc_cnt, i;
+    post_desc_t *ptr = NULL;
+    post_desc_t *next_phase = NULL;
+    uint8_t pos = plat_get_fru_sel();
+
+    /* Temporary return sample */
+    *next = 0xff;
+    *end = 0x00;
+    memcpy(buffer, "Hello Post", 10);
+    *length = 10;
+    return 0;
+}
+
+/* Need to implement this */
+int plat_udbg_get_gpio_desc(uint8_t index, uint8_t *next, uint8_t *level,
+                            uint8_t *def, uint8_t *count, uint8_t *buffer)
+{
+    return 0;
+}
+
+static int udbg_get_cri_sel(uint8_t frame, uint8_t page, uint8_t *next,
+                            uint8_t *count, uint8_t *buffer)
+{
+    int len;
+    int ret;
+    char line_buff[FRAME_PAGE_BUF_SIZE], *fptr;
+    const char *ptr;
+    FILE *fp;
+    struct stat file_stat;
+    uint8_t pos = plat_get_fru_sel();
+    static uint8_t pre_pos = FRU_ALL;
+    bool pos_changed = pre_pos != pos;
+
+    pre_pos = pos;
+
+    /* Revisit this */
+    fp = fopen("/mnt/data/cri_sel", "r");
+    if (fp)
+    {
+        if ((stat("/mnt/data/cri_sel", &file_stat) == 0) &&
+            (file_stat.st_mtime != frame_sel.mtime || pos_changed))
+        {
+            // initialize and clear frame
+            frame_sel.init(FRAME_BUFF_SIZE);
+            frame_sel.overwrite = 1;
+            frame_sel.max_page = 20;
+            frame_sel.mtime = file_stat.st_mtime;
+            snprintf(frame_sel.title, 32, "Cri SEL");
+
+            while (fgets(line_buff, FRAME_PAGE_BUF_SIZE, fp))
+            {
+                // Remove newline
+                line_buff[strlen(line_buff) - 1] = '\0';
+                ptr = line_buff;
+                // Find message
+                ptr = strstr(ptr, "local0.err");
+                if (ptr == NULL)
+                {
+                    continue;
+                }
+
+                if ((ptr = strrchr(ptr, ':')) == NULL)
+                {
+                    continue;
+                }
+                len = strlen(ptr);
+                if (len > 2)
+                {
+                    // to skip log string ": "
+                    ptr += 2;
+                }
+                // Write new message
+                frame_sel.insert(ptr, 0);
+            }
+        }
+        fclose(fp);
+    }
+    else
+    {
+        // Title only
+        frame_sel.init(FRAME_BUFF_SIZE);
+        snprintf(frame_sel.title, 32, "Cri SEL");
+        frame_sel.mtime = 0;
+    }
+
+    if (page > frame_sel.pages)
+    {
+        return -1;
+    }
+
+    ret = frame_sel.getPage(page, (char *)buffer, FRAME_PAGE_BUF_SIZE);
+    if (ret < 0)
+    {
+        *count = 0;
+        return -1;
+    }
+    *count = (uint8_t)ret;
+
+    if (page < frame_sel.pages)
+        *next = page + 1;
+    else
+        *next = 0xFF; // Set the value of next to 0xFF to indicate this is the
+                      // last page
+
+    return 0;
+}
+
+static int udbg_get_cri_sensor(uint8_t frame, uint8_t page, uint8_t *next,
+                               uint8_t *count, uint8_t *buffer)
+{
+    char str[32], temp_val[16], temp_thresh[8], print_format[32];
+    int i, ret;
+    float fvalue;
+    sensor_desc_t *cri_sensor = NULL;
+    size_t sensor_count = 0;
+    uint8_t pos = plat_get_fru_sel();
+    uint8_t fru;
+
+    if (page == 1)
+    {
+        // Only update frame data while getting page 1
+
+        // initialize and clear frame
+        frame_snr.init(FRAME_BUFF_SIZE);
+        snprintf(frame_snr.title, 32, "CriSensor");
+        frame_snr.append(str, 0);
+    } // End of update frame
+
+    if (page > frame_snr.pages)
+    {
+        return -1;
+    }
+
+    ret = frame_snr.getPage(page, (char *)buffer, FRAME_PAGE_BUF_SIZE);
+    if (ret < 0)
+    {
+        *count = 0;
+        return -1;
+    }
+    *count = (uint8_t)ret;
+
+    if (page < frame_snr.pages)
+        *next = page + 1;
+    else
+        *next = 0xFF; // Set the value of next to 0xFF to indicate this is the
+                      // last page
+
+    return 0;
+}
+
+static int udbg_get_info_page(uint8_t frame, uint8_t page, uint8_t *next,
+                              uint8_t *count, uint8_t *buffer)
+{
+    char line_buff[1000], *pres_dev = line_buff;
+    uint8_t pos = plat_get_fru_sel();
+    const char *delim = "\n";
+    int ret;
+    std::string serialName = "BOARD_SERIAL_NUMBER";
+    std::string partName = "BOARD_PART_NUMBER";
+    std::string verDel = "VERSION=";
+    std::string verPath = "/etc/os-release";
+
+    if (page == 1)
+    {
+        // Only update frame data while getting page 1
+
+        // initialize and clear frame
+        frame_info.init(FRAME_BUFF_SIZE);
+        snprintf(frame_info.title, 32, "SYS_Info");
+
+        // FRU TBD:
+        std::string data;
+        frame_info.append("SN:", 0);
+        if (getFruData(data, serialName) != 0)
+        {
+            data = "Not Found";
+        }
+        frame_info.append(data.c_str(), 1);
+        frame_info.append("PN:", 0);
+        if (getFruData(data, partName) != 0)
+        {
+            data = "Not Found";
+        }
+        frame_info.append(data.c_str(), 1);
+
+        // LAN
+        getNetworkData(3, line_buff);
+        frame_info.append("BMC_IP:", 0);
+        frame_info.append(line_buff, 1);
+        getNetworkData(59, line_buff);
+        frame_info.append("BMC_IPv6:", 0);
+        frame_info.append(line_buff, 1);
+
+        // BMC ver
+        std::ifstream file(verPath);
+        if (file)
+        {
+            std::string line;
+            while (std::getline(file, line))
+            {
+                if (line.find(verDel) != std::string::npos)
+                {
+                    std::string bmcVer = line.substr(verDel.size());
+                    frame_info.append("BMC_FW_ver:", 0);
+                    frame_info.append(bmcVer.c_str(), 1);
+                    break;
+                }
+            }
+        }
+
+        /* TBD: BIOS ver, ME status and Board ID needs implementation */
+        // BIOS ver
+
+        // ME status
+
+        // Board ID
+
+        // Battery - Use Escape sequence
+        frame_info.append("Battery:", 0);
+        frame_info.append(ESC_BAT "     ", 1);
+        // frame_info.append(&frame_info, esc_bat, 1);
+
+        // MCU Version - Use Escape sequence
+        frame_info.append("MCUbl_ver:", 0);
+        frame_info.append(ESC_MCU_BL_VER, 1);
+        frame_info.append("MCU_ver:", 0);
+        frame_info.append(ESC_MCU_RUN_VER, 1);
+
+        // TBD:
+        // Sys config present device
+
+    } // End of update frame
+
+    if (page > frame_info.pages)
+    {
+        return -1;
+    }
+
+    ret = frame_info.getPage(page, (char *)buffer, FRAME_PAGE_BUF_SIZE);
+    if (ret < 0)
+    {
+        *count = 0;
+        return -1;
+    }
+    *count = (uint8_t)ret;
+
+    if (page < frame_info.pages)
+        *next = page + 1;
+    else
+        *next = 0xFF; // Set the value of next to 0xFF to indicate this is the
+                      // last page
+
+    return 0;
+}
+
+int plat_udbg_get_frame_data(uint8_t frame, uint8_t page, uint8_t *next,
+                             uint8_t *count, uint8_t *buffer)
+{
+    switch (frame)
+    {
+        case 1: // info_page
+            return udbg_get_info_page(frame, page, next, count, buffer);
+        case 2: // critical SEL
+            return udbg_get_cri_sel(frame, page, next, count, buffer);
+        case 3: // critical Sensor
+            return udbg_get_cri_sensor(frame, page, next, count, buffer);
+        default:
+            return -1;
+    }
+}
+
+static uint8_t panel_main(uint8_t item)
+{
+    // Update item list when select item 0
+    switch (item)
+    {
+        case 1:
+            return panels[PANEL_BOOT_ORDER].select(0);
+        case 2:
+            return panels[PANEL_POWER_POLICY].select(0);
+        default:
+            return PANEL_MAIN;
+    }
+}
+
+static uint8_t panel_boot_order(uint8_t item)
+{
+    int i;
+    unsigned char buff[MAX_VALUE_LEN], pickup, len;
+    uint8_t pos = plat_get_fru_sel();
+
+    /* To be implemented */
+    /*
+  if (pos != FRU_ALL && pal_get_boot_order(pos, buff, buff, &len) == 0)
+  {
+  if (item > 0 && item < SIZE_BOOT_ORDER)
+  {
+  pickup = buff[item];
+  while (item > 1)
+  {
+    buff[item] = buff[item -1];
+    item--;
+  }
+  buff[item] = pickup;
+  buff[0] |= 0x80;
+  pal_set_boot_order(pos, buff, buff, &len);
+
+  // refresh items
+  return panels[PANEL_BOOT_ORDER].select(0);
+  }
+
+  // '*': boot flags valid, BIOS has not yet read
+  snprintf(panels[PANEL_BOOT_ORDER].item_str[0], 32,
+  "Boot Order%c", (buff[0] & 0x80)?'*':'\0');
+
+  for (i = 1; i < SIZE_BOOT_ORDER; i++)
+  {
+  switch (buff[i])
+  {
+    case 0x0:
+      snprintf(panels[PANEL_BOOT_ORDER].item_str[i], 32,
+        " USB device");
+      break;
+    case 0x1:
+      snprintf(panels[PANEL_BOOT_ORDER].item_str[i], 32,
+        " Network v4");
+      break;
+    case (0x1 | 0x8):
+      snprintf(panels[PANEL_BOOT_ORDER].item_str[i], 32,
+        " Network v6");
+      break;
+    case 0x2:
+      snprintf(panels[PANEL_BOOT_ORDER].item_str[i], 32,
+        " SATA HDD");
+      break;
+    case 0x3:
+      snprintf(panels[PANEL_BOOT_ORDER].item_str[i], 32,
+        " SATA-CDROM");
+      break;
+    case 0x4:
+      snprintf(panels[PANEL_BOOT_ORDER].item_str[i], 32,
+        " Other");
+      break;
+    default:
+      panels[PANEL_BOOT_ORDER].item_str[i][0] = '\0';
+      break;
+  }
+  }
+
+  // remove empty items
+  for (i--; (strlen(panels[PANEL_BOOT_ORDER].item_str[i]) == 0) && (i > 0); i--)
+  ;
+
+  panels[PANEL_BOOT_ORDER].item_num = i;
+  } else
+  {
+  panels[PANEL_BOOT_ORDER].item_num = 0;
+  }
+            */
+    return PANEL_BOOT_ORDER;
+}
+
+static uint8_t panel_power_policy(uint8_t item)
+{
+    uint8_t buff[32] = {0};
+    uint8_t res_len;
+    uint8_t pos = plat_get_fru_sel();
+    uint8_t policy;
+    //  uint8_t pwr_policy_item_map[3] = {POWER_CFG_ON, POWER_CFG_LPS,
+    //  POWER_CFG_OFF};
+
+    /* To be cleaned */
+    /*
+  if (pos != FRU_ALL) {
+  if (item > 0 && item <= sizeof(pwr_policy_item_map)) {
+  policy = pwr_policy_item_map[item - 1];
+  pal_set_power_restore_policy(pos, &policy, NULL);
+  }
+  pal_get_chassis_status(pos, NULL, buff, &res_len);
+  policy = (((uint8_t)buff[0]) >> 5) & 0x7;
+  snprintf(panels[PANEL_POWER_POLICY].item_str[1], 32,
+    "%cPower On", policy == POWER_CFG_ON ? '*' : ' ');
+  snprintf(panels[PANEL_POWER_POLICY].item_str[2], 32,
+    "%cLast State", policy == POWER_CFG_LPS ? '*' : ' ');
+  snprintf(panels[PANEL_POWER_POLICY].item_str[3], 32,
+    "%cPower Off", policy == POWER_CFG_OFF ? '*' : ' ');
+  panels[PANEL_POWER_POLICY].item_num = 3;
+  } else {
+  panels[PANEL_POWER_POLICY].item_num = 0;
+  }
+    */
+    return PANEL_POWER_POLICY;
+}
+
+int plat_udbg_control_panel(uint8_t panel, uint8_t operation, uint8_t item,
+                            uint8_t *count, uint8_t *buffer)
+{
+    if (panel > panelNum || panel < PANEL_MAIN)
+        return IPMI_CC_PARM_OUT_OF_RANGE;
+
+    // No more item; End of item list
+    if (item > panels[panel].item_num)
+        return IPMI_CC_PARM_OUT_OF_RANGE;
+
+    switch (operation)
+    {
+        case 0: // Get Description
+            break;
+        case 1: // Select item
+            panel = panels[panel].select(item);
+            item = 0;
+            break;
+        case 2: // Back
+            panel = panels[panel].parent;
+            item = 0;
+            break;
+        default:
+            return IPMI_CC_PARM_OUT_OF_RANGE;
+    }
+
+    buffer[0] = panel;
+    buffer[1] = item;
+    buffer[2] = strlen(panels[panel].item_str[item]);
+    if (buffer[2] > 0 && (buffer[2] + 3) < FRAME_PAGE_BUF_SIZE)
+    {
+        memcpy(&buffer[3], panels[panel].item_str[item], buffer[2]);
+    }
+    *count = buffer[2] + 3;
+    return IPMI_CC_OK;
+}
+
+} // end of namespace ipmi
diff --git a/src/utils.cpp b/src/utils.cpp
new file mode 100644
index 0000000..49c37ab
--- /dev/null
+++ b/src/utils.cpp
@@ -0,0 +1,504 @@
+#include "phosphor-ipmi-host/utils.hpp"
+
+#include <arpa/inet.h>
+#include <dirent.h>
+#include <net/if.h>
+
+#include <cstring>
+#include <iostream>
+#include <ipmid/api.h>
+#include <phosphor-logging/elog-errors.hpp>
+#include <phosphor-logging/log.hpp>
+#include <xyz/openbmc_project/Common/error.hpp>
+
+namespace ipmi
+{
+
+using namespace phosphor::logging;
+using namespace sdbusplus::xyz::openbmc_project::Common::Error;
+
+// Parameters
+enum class LanParam : uint8_t
+{
+    INPROGRESS = 0,
+    AUTHSUPPORT = 1,
+    AUTHENABLES = 2,
+    IP = 3,
+    IPSRC = 4,
+    MAC = 5,
+    SUBNET = 6,
+    GATEWAY = 12,
+    VLAN = 20,
+    CIPHER_SUITE_COUNT = 22,
+    CIPHER_SUITE_ENTRIES = 23,
+    IPV6 = 59,
+};
+
+namespace network
+{
+
+/** @brief checks if the given ip is Link Local Ip or not.
+ *  @param[in] ipaddress - IPAddress.
+ */
+bool isLinkLocalIP(const std::string &address)
+{
+    return address.find(IPV4_PREFIX) == 0 || address.find(IPV6_PREFIX) == 0;
+}
+
+} // namespace network
+
+// TODO There may be cases where an interface is implemented by multiple
+//  objects,to handle such cases we are interested on that object
+//  which are on interested busname.
+//  Currently mapper doesn't give the readable busname(gives busid) so we can't
+//  use busname to find the object,will do later once the support is there.
+
+DbusObjectInfo getDbusObject(sdbusplus::bus::bus &bus,
+                             const std::string &interface,
+                             const std::string &serviceRoot,
+                             const std::string &match)
+{
+    std::vector<DbusInterface> interfaces;
+    interfaces.emplace_back(interface);
+
+    auto depth = 0;
+
+    auto mapperCall = bus.new_method_call(MAPPER_BUS_NAME, MAPPER_OBJ,
+                                          MAPPER_INTF, "GetSubTree");
+
+    mapperCall.append(serviceRoot, depth, interfaces);
+
+    ObjectTree objectTree;
+    try
+    {
+        auto mapperReply = bus.call(mapperCall);
+        mapperReply.read(objectTree);
+    }
+    catch (sdbusplus::exception_t &)
+    {
+        log<level::ERR>("Error in mapper call");
+        elog<InternalFailure>();
+    }
+
+    if (objectTree.empty())
+    {
+        log<level::ERR>("No Object has implemented the interface",
+                        entry("INTERFACE=%s", interface.c_str()));
+        elog<InternalFailure>();
+    }
+
+    DbusObjectInfo objectInfo;
+
+    // if match is empty then return the first object
+    if (match == "")
+    {
+        objectInfo = std::make_pair(
+            objectTree.begin()->first,
+            std::move(objectTree.begin()->second.begin()->first));
+        return objectInfo;
+    }
+
+    // else search the match string in the object path
+    auto objectFound = false;
+    for (auto &object : objectTree)
+    {
+        if (object.first.find(match) != std::string::npos)
+        {
+            objectFound = true;
+            objectInfo = make_pair(object.first,
+                                   std::move(object.second.begin()->first));
+            break;
+        }
+    }
+
+    if (!objectFound)
+    {
+        log<level::ERR>("Failed to find object which matches",
+                        entry("MATCH=%s", match.c_str()));
+        elog<InternalFailure>();
+    }
+    return objectInfo;
+}
+
+DbusObjectInfo getIPObject(sdbusplus::bus::bus &bus,
+                           const std::string &interface,
+                           const std::string &serviceRoot,
+                           const std::string &match)
+{
+    auto objectTree = getAllDbusObjects(bus, serviceRoot, interface, match);
+
+    if (objectTree.empty())
+    {
+        log<level::ERR>("No Object has implemented the IP interface",
+                        entry("INTERFACE=%s", interface.c_str()));
+        elog<InternalFailure>();
+    }
+
+    DbusObjectInfo objectInfo;
+
+    for (auto &object : objectTree)
+    {
+        auto variant = ipmi::getDbusProperty(
+            bus, object.second.begin()->first, object.first,
+            ipmi::network::IP_INTERFACE, "Address");
+
+        objectInfo = std::make_pair(object.first, object.second.begin()->first);
+
+        // if LinkLocalIP found look for Non-LinkLocalIP
+        if (ipmi::network::isLinkLocalIP(
+                sdbusplus::message::variant_ns::get<std::string>(variant)))
+        {
+            continue;
+        }
+        else
+        {
+            break;
+        }
+    }
+    return objectInfo;
+}
+
+Value getDbusProperty(sdbusplus::bus::bus &bus, const std::string &service,
+                      const std::string &objPath, const std::string &interface,
+                      const std::string &property)
+{
+
+    Value value;
+
+    auto method = bus.new_method_call(service.c_str(), objPath.c_str(),
+                                      PROP_INTF, METHOD_GET);
+
+    method.append(interface, property);
+
+    try
+    {
+        auto reply = bus.call(method);
+        reply.read(value);
+    }
+    catch (sdbusplus::exception_t &)
+    {
+        log<level::ERR>("Failed to get property",
+                        entry("PROPERTY=%s", property.c_str()),
+                        entry("PATH=%s", objPath.c_str()),
+                        entry("INTERFACE=%s", interface.c_str()));
+        elog<InternalFailure>();
+    }
+
+    return value;
+}
+
+PropertyMap getAllDbusProperties(sdbusplus::bus::bus &bus,
+                                 const std::string &service,
+                                 const std::string &objPath,
+                                 const std::string &interface)
+{
+    PropertyMap properties;
+
+    auto method = bus.new_method_call(service.c_str(), objPath.c_str(),
+                                      PROP_INTF, METHOD_GET_ALL);
+
+    method.append(interface);
+
+    try
+    {
+        auto reply = bus.call(method);
+        reply.read(properties);
+    }
+    catch (sdbusplus::exception_t &)
+    {
+        log<level::ERR>("Failed to get all properties",
+                        entry("PATH=%s", objPath.c_str()),
+                        entry("INTERFACE=%s", interface.c_str()));
+        elog<InternalFailure>();
+    }
+
+    return properties;
+}
+
+ObjectValueTree getManagedObjects(sdbusplus::bus::bus &bus,
+                                  const std::string &service,
+                                  const std::string &objPath)
+{
+    ipmi::ObjectValueTree interfaces;
+
+    auto method = bus.new_method_call(service.c_str(), objPath.c_str(),
+                                      "org.freedesktop.DBus.ObjectManager",
+                                      "GetManagedObjects");
+
+    try
+    {
+        auto reply = bus.call(method);
+        reply.read(interfaces);
+    }
+    catch (sdbusplus::exception_t &)
+    {
+        log<level::ERR>("Failed to get managed objects",
+                        entry("PATH=%s", objPath.c_str()));
+        elog<InternalFailure>();
+    }
+
+    return interfaces;
+}
+
+void setDbusProperty(sdbusplus::bus::bus &bus, const std::string &service,
+                     const std::string &objPath, const std::string &interface,
+                     const std::string &property, const Value &value)
+{
+    auto method = bus.new_method_call(service.c_str(), objPath.c_str(),
+                                      PROP_INTF, METHOD_SET);
+
+    method.append(interface, property, value);
+
+    try
+    {
+        bus.call(method);
+    }
+    catch (sdbusplus::exception_t &)
+    {
+        log<level::ERR>("Failed to set property",
+                        entry("PROPERTY=%s", property.c_str()),
+                        entry("PATH=%s", objPath.c_str()),
+                        entry("INTERFACE=%s", interface.c_str()));
+        elog<InternalFailure>();
+    }
+}
+
+ServiceCache::ServiceCache(const std::string &intf, const std::string &path) :
+    intf(intf), path(path), cachedService(std::nullopt),
+    cachedBusName(std::nullopt)
+{
+}
+
+ServiceCache::ServiceCache(std::string &&intf, std::string &&path) :
+    intf(std::move(intf)), path(std::move(path)), cachedService(std::nullopt),
+    cachedBusName(std::nullopt)
+{
+}
+
+const std::string &ServiceCache::getService(sdbusplus::bus::bus &bus)
+{
+    if (!isValid(bus))
+    {
+        cachedBusName = bus.get_unique_name();
+        cachedService = ::ipmi::getService(bus, intf, path);
+    }
+    return cachedService.value();
+}
+
+void ServiceCache::invalidate()
+{
+    cachedBusName = std::nullopt;
+    cachedService = std::nullopt;
+}
+
+sdbusplus::message::message
+    ServiceCache::newMethodCall(sdbusplus::bus::bus &bus, const char *intf,
+                                const char *method)
+{
+    return bus.new_method_call(getService(bus).c_str(), path.c_str(), intf,
+                               method);
+}
+
+bool ServiceCache::isValid(sdbusplus::bus::bus &bus) const
+{
+    return cachedService && cachedBusName == bus.get_unique_name();
+}
+
+std::string getService(sdbusplus::bus::bus &bus, const std::string &intf,
+                       const std::string &path)
+{
+    auto mapperCall =
+        bus.new_method_call("xyz.openbmc_project.ObjectMapper",
+                            "/xyz/openbmc_project/object_mapper",
+                            "xyz.openbmc_project.ObjectMapper", "GetObject");
+
+    mapperCall.append(path);
+    mapperCall.append(std::vector<std::string>({intf}));
+
+    std::map<std::string, std::vector<std::string>> mapperResponse;
+    try
+    {
+        auto mapperResponseMsg = bus.call(mapperCall);
+        mapperResponseMsg.read(mapperResponse);
+    }
+    catch (sdbusplus::exception_t &)
+    {
+        throw std::runtime_error("ERROR in mapper call");
+    }
+
+    if (mapperResponse.begin() == mapperResponse.end())
+    {
+        throw std::runtime_error("ERROR in reading the mapper response");
+    }
+
+    return mapperResponse.begin()->first;
+}
+
+ipmi::ObjectTree getAllDbusObjects(sdbusplus::bus::bus &bus,
+                                   const std::string &serviceRoot,
+                                   const std::string &interface,
+                                   const std::string &match)
+{
+    std::vector<std::string> interfaces;
+    interfaces.emplace_back(interface);
+
+    auto depth = 0;
+
+    auto mapperCall = bus.new_method_call(MAPPER_BUS_NAME, MAPPER_OBJ,
+                                          MAPPER_INTF, "GetSubTree");
+
+    mapperCall.append(serviceRoot, depth, interfaces);
+
+    ObjectTree objectTree;
+    try
+    {
+        auto mapperReply = bus.call(mapperCall);
+        mapperReply.read(objectTree);
+    }
+    catch (sdbusplus::exception_t &)
+    {
+        log<level::ERR>("Error in mapper call",
+                        entry("SERVICEROOT=%s", serviceRoot.c_str()),
+                        entry("INTERFACE=%s", interface.c_str()));
+
+        elog<InternalFailure>();
+    }
+
+    for (auto it = objectTree.begin(); it != objectTree.end();)
+    {
+        if (it->first.find(match) == std::string::npos)
+        {
+            it = objectTree.erase(it);
+        }
+        else
+        {
+            ++it;
+        }
+    }
+
+    return objectTree;
+}
+
+void deleteAllDbusObjects(sdbusplus::bus::bus &bus,
+                          const std::string &serviceRoot,
+                          const std::string &interface,
+                          const std::string &match)
+{
+    try
+    {
+        auto objectTree = getAllDbusObjects(bus, serviceRoot, interface, match);
+
+        for (auto &object : objectTree)
+        {
+            method_no_args::callDbusMethod(bus, object.second.begin()->first,
+                                           object.first, DELETE_INTERFACE,
+                                           "Delete");
+        }
+    }
+    catch (sdbusplus::exception::exception &e)
+    {
+        log<level::INFO>("sdbusplus exception - Unable to delete the objects",
+                         entry("ERROR=%s", e.what()),
+                         entry("INTERFACE=%s", interface.c_str()),
+                         entry("SERVICE=%s", serviceRoot.c_str()));
+    }
+}
+
+namespace variant_ns = sdbusplus::message::variant_ns;
+ipmi_ret_t getNetworkData(uint8_t lan_param, char *data)
+{
+    ipmi_ret_t rc = IPMI_CC_OK;
+    sdbusplus::bus::bus bus(ipmid_get_sd_bus_connection());
+
+    const std::string ethdevice = "eth0";
+
+    switch (static_cast<LanParam>(lan_param))
+    {
+        case LanParam::IP:
+        {
+            auto ethIP = ethdevice + "/" + ipmi::network::IP_TYPE;
+            std::string ipaddress;
+            auto ipObjectInfo = ipmi::getIPObject(
+                bus, ipmi::network::IP_INTERFACE, ipmi::network::ROOT, ethIP);
+
+            auto properties = ipmi::getAllDbusProperties(
+                bus, ipObjectInfo.second, ipObjectInfo.first,
+                ipmi::network::IP_INTERFACE);
+
+            ipaddress = variant_ns::get<std::string>(properties["Address"]);
+
+            std::strcpy(data, ipaddress.c_str());
+        }
+        break;
+
+        case LanParam::IPV6:
+        {
+            auto ethIP = ethdevice + "/ipv6";
+            std::string ipaddress;
+            auto ipObjectInfo = ipmi::getIPObject(
+                bus, ipmi::network::IP_INTERFACE, ipmi::network::ROOT, ethIP);
+
+            auto properties = ipmi::getAllDbusProperties(
+                bus, ipObjectInfo.second, ipObjectInfo.first,
+                ipmi::network::IP_INTERFACE);
+
+            ipaddress = variant_ns::get<std::string>(properties["Address"]);
+
+            std::strcpy(data, ipaddress.c_str());
+        }
+        break;
+
+        case LanParam::MAC:
+        {
+            std::string macAddress;
+            auto macObjectInfo =
+                ipmi::getDbusObject(bus, ipmi::network::MAC_INTERFACE,
+                                    ipmi::network::ROOT, ethdevice);
+
+            auto variant = ipmi::getDbusProperty(
+                bus, macObjectInfo.second, macObjectInfo.first,
+                ipmi::network::MAC_INTERFACE, "MACAddress");
+
+            macAddress = variant_ns::get<std::string>(variant);
+
+            sscanf(macAddress.c_str(), ipmi::network::MAC_ADDRESS_FORMAT,
+                   (data), (data + 1), (data + 2), (data + 3), (data + 4),
+                   (data + 5));
+            std::strcpy(data, macAddress.c_str());
+        }
+        break;
+
+        default:
+            rc = IPMI_CC_PARM_OUT_OF_RANGE;
+    }
+    return rc;
+}
+
+namespace method_no_args
+{
+
+void callDbusMethod(sdbusplus::bus::bus &bus, const std::string &service,
+                    const std::string &objPath, const std::string &interface,
+                    const std::string &method)
+
+{
+    auto busMethod = bus.new_method_call(service.c_str(), objPath.c_str(),
+                                         interface.c_str(), method.c_str());
+
+    try
+    {
+        bus.call(busMethod);
+    }
+    catch (sdbusplus::exception_t &)
+    {
+        log<level::ERR>("Failed to execute method",
+                        entry("METHOD=%s", method.c_str()),
+                        entry("PATH=%s", objPath.c_str()),
+                        entry("INTERFACE=%s", interface.c_str()));
+        elog<InternalFailure>();
+    }
+}
+
+} // namespace method_no_args
+
+} // namespace ipmi