Implement basic function of Cold Redundancy

Implement basic function of Cold Redundancy include rotation, config,
check and reRanking.

Tested:
Change rotationPeriod to a smaller value such as 60 seconds. Insert
two PSU on system, and check PSU rank order register 0xd0. After 60s
The value of rank order in PSU1 will change to 2, and the value of
rank order in PSU2 will change to 1.
If removed AC Cable on one PSU. The rank order in this PSU will be
always 0, and the rank order in the other PSU will be always 1.

Change-Id: Idc14f783ec83db005ecf9fc53332431ba6a6daee
Signed-off-by: Cheng C Yang <cheng.c.yang@intel.com>

6 files changed, 354 insertions, 11 deletions

diff --git a/psu-manager/CMakeLists.txt b/psu-manager/CMakeLists.txt
index d29d98a..760ed81 100644
--- a/psu-manager/CMakeLists.txt
+++ b/psu-manager/CMakeLists.txt
@@ -13,7 +13,7 @@ include_directories (
 
 project (psumanager CXX)
 
-set (PSU_CR_SRC_FILES src/utility.cpp src/coldRedundancy.cpp)
+set (PSU_CR_SRC_FILES src/utility.cpp src/cold_redundancy.cpp)
 
 set (EXTERNAL_PACKAGES Boost sdbusplus-project nlohmann-json)
 set (CR_LINK_LIBS -lsystemd stdc++fs sdbusplus)
@@ -35,7 +35,7 @@ link_directories (${SDBUSPLUSPLUS_LIBRARY_DIRS})
 include_directories (${DBUSINTERFACE_INCLUDE_DIRS})
 link_directories (${DBUSINTERFACE_LIBRARY_DIRS})
 
-add_executable (psuredundancy ${PSU_CR_SRC_FILES} src/redundancyMain.cpp)
+add_executable (psuredundancy ${PSU_CR_SRC_FILES} src/redundancy_main.cpp)
 add_dependencies (psuredundancy sdbusplus-project)
 target_link_libraries (psuredundancy ${CR_LINK_LIBS})
 target_link_libraries (psuredundancy i2c)
diff --git a/psu-manager/include/coldRedundancy.hpp b/psu-manager/include/cold_redundancy.hpp
index 9eba4e9..53bf679 100644
--- a/psu-manager/include/coldRedundancy.hpp
+++ b/psu-manager/include/cold_redundancy.hpp
@@ -47,11 +47,21 @@ class ColdRedundancy
     uint8_t numberOfPSU = 0;
     uint32_t rotationPeriod = 7 * secondsInOneDay;
 
+    void startRotateCR(void);
+    void startCRCheck(void);
+    void rotateCR(void);
+    void configCR(bool reConfig);
+    void checkCR(void);
+    void reRanking(void);
+    void putWarmRedundant();
+
     std::shared_ptr<sdbusplus::asio::connection>& systemBus;
 
     std::vector<std::unique_ptr<sdbusplus::bus::match::match>> matches;
-    boost::asio::deadline_timer timerRotation;
-    boost::asio::deadline_timer timerCheck;
+    boost::asio::steady_timer timerRotation;
+    boost::asio::steady_timer timerCheck;
+    boost::asio::steady_timer warmRedundantTimer1;
+    boost::asio::steady_timer warmRedundantTimer2;
 };
 
 constexpr const uint8_t pmbusCmdCRSupport = 0xd0;
diff --git a/psu-manager/include/utility.hpp b/psu-manager/include/utility.hpp
index 0b4d834..342d36f 100644
--- a/psu-manager/include/utility.hpp
+++ b/psu-manager/include/utility.hpp
@@ -29,8 +29,6 @@ using BasicVariantType =
     std::variant<std::vector<std::string>, std::string, int64_t, uint64_t,
                  double, int32_t, uint32_t, int16_t, uint16_t, uint8_t, bool>;
 
-// using PropertyMapType =
-//    std::vector<std::pair<std::string, BasicVariantType>>;
 using PropertyMapType =
     boost::container::flat_map<std::string, BasicVariantType>;
 
@@ -54,3 +52,6 @@ void getPSUEvent(
     const std::array<const char*, 1>& type,
     const std::shared_ptr<sdbusplus::asio::connection>& dbusConnection,
     const std::string& psuName, PSUState& state);
+
+int i2cSet(uint8_t bus, uint8_t slaveAddr, uint8_t regAddr, uint8_t value);
+int i2cGet(uint8_t bus, uint8_t slaveAddr, uint8_t regAddr, uint8_t& value);
diff --git a/psu-manager/src/coldRedundancy.cpp b/psu-manager/src/cold_redundancy.cpp
index 308d17a..118627d 100644
--- a/psu-manager/src/coldRedundancy.cpp
+++ b/psu-manager/src/cold_redundancy.cpp
@@ -18,7 +18,7 @@
 #include <boost/algorithm/string/predicate.hpp>
 #include <boost/algorithm/string/replace.hpp>
 #include <boost/container/flat_set.hpp>
-#include <coldRedundancy.hpp>
+#include <cold_redundancy.hpp>
 #include <filesystem>
 #include <fstream>
 #include <iostream>
@@ -45,7 +45,8 @@ ColdRedundancy::ColdRedundancy(
     std::shared_ptr<sdbusplus::asio::connection>& systemBus) :
     sdbusplus::xyz::openbmc_project::Control::server::PowerSupplyRedundancy(
         *systemBus, coldRedundancyPath),
-    timerRotation(io), timerCheck(io), systemBus(systemBus)
+    timerRotation(io), timerCheck(io), systemBus(systemBus),
+    warmRedundantTimer1(io), warmRedundantTimer2(io)
 {
     io.post([this, &io, &objectServer, &systemBus]() {
         createPSU(io, objectServer, systemBus);
@@ -58,8 +59,8 @@ ColdRedundancy::ColdRedundancy(
                 std::cerr << "callback method error\n";
                 return;
             }
-            boost::asio::deadline_timer filterTimer(io);
-            filterTimer.expires_from_now(boost::posix_time::seconds(1));
+            boost::asio::steady_timer filterTimer(io);
+            filterTimer.expires_after(std::chrono::seconds(1));
             filterTimer.async_wait([this, &io, &objectServer, &systemBus](
                                        const boost::system::error_code& ec) {
                 if (ec == boost::asio::error::operation_aborted)
@@ -184,6 +185,8 @@ void ColdRedundancy::createPSU(
         "xyz.openbmc_project.ObjectMapper", "GetSubTree",
         "/xyz/openbmc_project/inventory/system/powersupply", 2,
         psuInterfaceTypes);
+    startRotateCR();
+    startCRCheck();
 }
 
 PowerSupply::PowerSupply(
@@ -199,6 +202,216 @@ PowerSupply::PowerSupply(
     }
 }
 
+// Reranking PSU orders with ascending order, if any of the PSU is not in
+// normal state, changing rotation algo to bmc specific, and Reranking all
+// other normal PSU. If all PSU are in normal state, and rotation algo is
+// user specific, do nothing.
+void ColdRedundancy::reRanking(void)
+{
+    uint8_t index = 1;
+    if (rotationAlgo == bmcSpecific)
+    {
+        for (auto& psu : powerSupplies)
+        {
+            if (psu->state == PSUState::normal)
+            {
+                psu->order = (index++);
+            }
+            else
+            {
+                psu->order = 0;
+            }
+        }
+    }
+    else
+    {
+        for (auto& psu : powerSupplies)
+        {
+            if (psu->state == PSUState::acLost)
+            {
+                rotationAlgo = bmcSpecific;
+                reRanking();
+                return;
+            }
+        }
+    }
+}
+
+void ColdRedundancy::configCR(bool reConfig)
+{
+    if (!crSupported || !crEnabled)
+    {
+        return;
+    }
+    putWarmRedundant();
+    warmRedundantTimer2.expires_after(std::chrono::seconds(5));
+    warmRedundantTimer2.async_wait([this, reConfig](
+                                       const boost::system::error_code& ec) {
+        if (ec == boost::asio::error::operation_aborted)
+        {
+            return;
+        }
+        else if (ec)
+        {
+            std::cerr << "warm redundant timer error\n";
+            return;
+        }
+
+        if (reConfig)
+        {
+            reRanking();
+        }
+
+        for (auto& psu : powerSupplies)
+        {
+            if (psu->state == PSUState::normal && psu->order != 0)
+            {
+                if (i2cSet(psu->bus, psu->address, pmbusCmdCRSupport,
+                           psu->order))
+                {
+                    std::cerr << "Failed to change PSU Cold Redundancy order\n";
+                }
+            }
+        }
+    });
+}
+
+void ColdRedundancy::checkCR(void)
+{
+    if (!crSupported)
+    {
+        return;
+    }
+    if (!crEnabled)
+    {
+        putWarmRedundant();
+        return;
+    }
+
+    for (auto& psu : powerSupplies)
+    {
+        if (psu->state == PSUState::normal)
+        {
+            uint8_t order = 0;
+            if (i2cGet(psu->bus, psu->address, pmbusCmdCRSupport, order))
+            {
+                std::cerr << "Failed to get PSU Cold Redundancy order\n";
+                continue;
+            }
+            if (order == 0)
+            {
+                configCR(true);
+                return;
+            }
+        }
+    }
+}
+
+void ColdRedundancy::startCRCheck()
+{
+    timerCheck.expires_after(std::chrono::seconds(60));
+    timerCheck.async_wait([this](const boost::system::error_code& ec) {
+        if (ec == boost::asio::error::operation_aborted)
+        {
+            return;
+        }
+        else if (ec)
+        {
+            std::cerr << "timer error\n";
+        }
+        if (crSupported)
+        {
+            checkCR();
+        }
+        startCRCheck();
+    });
+}
+
+// Rotate the orders of PSU redundancy. Each normal PSU will add one to its
+// rank order. And the PSU with last rank order will become the rank order 1
+void ColdRedundancy::rotateCR(void)
+{
+    if (!crSupported || !crEnabled)
+    {
+        return;
+    }
+    putWarmRedundant();
+    warmRedundantTimer1.expires_after(std::chrono::seconds(5));
+    warmRedundantTimer1.async_wait([this](const boost::system::error_code& ec) {
+        if (ec == boost::asio::error::operation_aborted)
+        {
+            return;
+        }
+        else if (ec)
+        {
+            std::cerr << "warm redundant timer error\n";
+            return;
+        }
+
+        int goodPSUCount = 0;
+
+        for (auto& psu : powerSupplies)
+        {
+            if (psu->state == PSUState::normal)
+            {
+                goodPSUCount++;
+            }
+        }
+
+        for (auto& psu : powerSupplies)
+        {
+            if (psu->order == 0)
+            {
+                continue;
+            }
+            psu->order++;
+            if (psu->order > goodPSUCount)
+            {
+                psu->order = 1;
+            }
+            if (i2cSet(psu->bus, psu->address, pmbusCmdCRSupport, psu->order))
+            {
+                std::cerr << "Failed to change PSU Cold Redundancy order\n";
+            }
+        }
+    });
+}
+
+void ColdRedundancy::startRotateCR()
+{
+    timerRotation.expires_after(std::chrono::seconds(rotationPeriod));
+    timerRotation.async_wait([this](const boost::system::error_code& ec) {
+        if (ec == boost::asio::error::operation_aborted)
+        {
+            return;
+        }
+        else if (ec)
+        {
+            std::cerr << "timer error\n";
+        }
+        if (crSupported && rotationEnabled)
+        {
+            rotateCR();
+        }
+        startRotateCR();
+    });
+}
+
+void ColdRedundancy::putWarmRedundant(void)
+{
+    if (!crSupported)
+    {
+        return;
+    }
+    for (auto& psu : powerSupplies)
+    {
+        if (psu->state == PSUState::normal)
+        {
+            i2cSet(psu->bus, psu->address, pmbusCmdCRSupport, 0);
+        }
+    }
+}
+
 PowerSupply::~PowerSupply()
 {
 }
diff --git a/psu-manager/src/redundancyMain.cpp b/psu-manager/src/redundancy_main.cpp
index fa147c5..7bae05b 100644
--- a/psu-manager/src/redundancyMain.cpp
+++ b/psu-manager/src/redundancy_main.cpp
@@ -14,7 +14,7 @@
 // limitations under the License.
 */
 
-#include <coldRedundancy.hpp>
+#include <cold_redundancy.hpp>
 #include <sdbusplus/asio/object_server.hpp>
 
 int main(int argc, char** argv)
diff --git a/psu-manager/src/utility.cpp b/psu-manager/src/utility.cpp
index 0b9581e..2143c80 100644
--- a/psu-manager/src/utility.cpp
+++ b/psu-manager/src/utility.cpp
@@ -24,6 +24,125 @@ extern "C" {
 #include <linux/i2c-dev.h>
 }
 
+int i2cSet(uint8_t bus, uint8_t slaveAddr, uint8_t regAddr, uint8_t value)
+{
+    unsigned long funcs = 0;
+    std::string devPath = "/dev/i2c-" + std::to_string(bus);
+
+    int fd = ::open(devPath.c_str(), O_RDWR);
+    if (fd < 0)
+    {
+        phosphor::logging::log<phosphor::logging::level::ERR>(
+            "Error in open!",
+            phosphor::logging::entry("PATH=%s", devPath.c_str()),
+            phosphor::logging::entry("SLAVEADDR=0x%x", slaveAddr));
+        return -1;
+    }
+
+    if (::ioctl(fd, I2C_FUNCS, &funcs) < 0)
+    {
+        phosphor::logging::log<phosphor::logging::level::ERR>(
+            "Error in I2C_FUNCS!",
+            phosphor::logging::entry("PATH=%s", devPath.c_str()),
+            phosphor::logging::entry("SLAVEADDR=0x%x", slaveAddr));
+        ::close(fd);
+        return -1;
+    }
+
+    if (!(funcs & I2C_FUNC_SMBUS_WRITE_BYTE_DATA))
+    {
+        phosphor::logging::log<phosphor::logging::level::ERR>(
+            "i2c bus does not support write!",
+            phosphor::logging::entry("PATH=%s", devPath.c_str()),
+            phosphor::logging::entry("SLAVEADDR=0x%x", slaveAddr));
+        ::close(fd);
+        return -1;
+    }
+
+    if (::ioctl(fd, I2C_SLAVE_FORCE, slaveAddr) < 0)
+    {
+        phosphor::logging::log<phosphor::logging::level::ERR>(
+            "Error in I2C_SLAVE_FORCE!",
+            phosphor::logging::entry("PATH=%s", devPath.c_str()),
+            phosphor::logging::entry("SLAVEADDR=0x%x", slaveAddr));
+        ::close(fd);
+        return -1;
+    }
+
+    if (::i2c_smbus_write_byte_data(fd, regAddr, value) < 0)
+    {
+        phosphor::logging::log<phosphor::logging::level::ERR>(
+            "Error in i2c write!",
+            phosphor::logging::entry("PATH=%s", devPath.c_str()),
+            phosphor::logging::entry("SLAVEADDR=0x%x", slaveAddr));
+        ::close(fd);
+        return -1;
+    }
+
+    ::close(fd);
+    return 0;
+}
+
+int i2cGet(uint8_t bus, uint8_t slaveAddr, uint8_t regAddr, uint8_t& value)
+{
+    unsigned long funcs = 0;
+    std::string devPath = "/dev/i2c-" + std::to_string(bus);
+
+    int fd = ::open(devPath.c_str(), O_RDWR);
+    if (fd < 0)
+    {
+        phosphor::logging::log<phosphor::logging::level::ERR>(
+            "Error in open!",
+            phosphor::logging::entry("PATH=%s", devPath.c_str()),
+            phosphor::logging::entry("SLAVEADDR=0x%x", slaveAddr));
+        return -1;
+    }
+
+    if (::ioctl(fd, I2C_FUNCS, &funcs) < 0)
+    {
+        phosphor::logging::log<phosphor::logging::level::ERR>(
+            "Error in I2C_FUNCS!",
+            phosphor::logging::entry("PATH=%s", devPath.c_str()),
+            phosphor::logging::entry("SLAVEADDR=0x%x", slaveAddr));
+        ::close(fd);
+        return -1;
+    }
+
+    if (!(funcs & I2C_FUNC_SMBUS_READ_BYTE_DATA))
+    {
+        phosphor::logging::log<phosphor::logging::level::ERR>(
+            "i2c bus does not support read!",
+            phosphor::logging::entry("PATH=%s", devPath.c_str()),
+            phosphor::logging::entry("SLAVEADDR=0x%x", slaveAddr));
+        ::close(fd);
+        return -1;
+    }
+
+    if (::ioctl(fd, I2C_SLAVE_FORCE, slaveAddr) < 0)
+    {
+        phosphor::logging::log<phosphor::logging::level::ERR>(
+            "Error in I2C_SLAVE_FORCE!",
+            phosphor::logging::entry("PATH=%s", devPath.c_str()),
+            phosphor::logging::entry("SLAVEADDR=0x%x", slaveAddr));
+        ::close(fd);
+        return -1;
+    }
+
+    value = ::i2c_smbus_read_byte_data(fd, regAddr);
+    if (value < 0)
+    {
+        phosphor::logging::log<phosphor::logging::level::ERR>(
+            "Error in i2c read!",
+            phosphor::logging::entry("PATH=%s", devPath.c_str()),
+            phosphor::logging::entry("SLAVEADDR=0x%x", slaveAddr));
+        ::close(fd);
+        return -1;
+    }
+
+    ::close(fd);
+    return 0;
+}
+
 void getPSUEvent(const std::array<const char*, 1>& configTypes,
                  const std::shared_ptr<sdbusplus::asio::connection>& conn,
                  const std::string& psuName, PSUState& state)