Standardize read errors

Each sensor handled read errors their own way,
making it inconsistant. This helps to align them all
in the base class, so that error handling happens the
same for each sensor. It also aligns the power state
change handling.

Tested: Tested DC Cycling and Enabling/Disabling ME to
make sure functional change correctly

Change-Id: I1a191d27629602e1ca3871d933af07b15bf9f331
Signed-off-by: James Feist <james.feist@linux.intel.com>
diff --git a/include/ADCSensor.hpp b/include/ADCSensor.hpp
index d4ad601..71b92da 100644
--- a/include/ADCSensor.hpp
+++ b/include/ADCSensor.hpp
@@ -78,10 +78,8 @@
     boost::asio::deadline_timer waitTimer;
     std::shared_ptr<boost::asio::streambuf> readBuf;
     std::string path;
-    size_t errCount;
     double scaleFactor;
     std::optional<BridgeGpio> bridgeGpio;
-    PowerState readState;
     thresholds::ThresholdTimer thresholdTimer;
     void handleResponse(const boost::system::error_code& err);
     void checkThresholds(void) override;
diff --git a/include/CPUSensor.hpp b/include/CPUSensor.hpp
index 29cd54a..4cad880 100644
--- a/include/CPUSensor.hpp
+++ b/include/CPUSensor.hpp
@@ -44,7 +44,6 @@
     double privTcontrol;
     double dtsOffset;
     bool show;
-    size_t errCount;
     void setupRead(void);
     void handleResponse(const boost::system::error_code& err);
     void checkThresholds(void) override;
diff --git a/include/HwmonTempSensor.hpp b/include/HwmonTempSensor.hpp
index 7da2a4d..a35cb90 100644
--- a/include/HwmonTempSensor.hpp
+++ b/include/HwmonTempSensor.hpp
@@ -29,7 +29,6 @@
     boost::asio::deadline_timer waitTimer;
     boost::asio::streambuf readBuf;
     std::string path;
-    PowerState readState;
     size_t errCount;
 
     void handleResponse(const boost::system::error_code& err);
diff --git a/include/IpmbSensor.hpp b/include/IpmbSensor.hpp
index 3d78bfa..e1b1ddb 100644
--- a/include/IpmbSensor.hpp
+++ b/include/IpmbSensor.hpp
@@ -52,8 +52,7 @@
     void init(void);
     void loadDefaults(void);
     void runInitCmd(void);
-    void processError(void);
-    double processReading(const std::vector<uint8_t>& data);
+    bool processReading(const std::vector<uint8_t>& data, double& resp);
 
     IpmbType type;
     IpmbSubType subType;
@@ -68,8 +67,6 @@
     std::optional<uint8_t> initCommand;
     std::vector<uint8_t> initData;
 
-    // to date all ipmb sensors are power on only
-    PowerState readState;
     ReadingFormat readingFormat;
 
   private:
diff --git a/include/NVMeSensor.hpp b/include/NVMeSensor.hpp
index ec0bfd1..132ef29 100644
--- a/include/NVMeSensor.hpp
+++ b/include/NVMeSensor.hpp
@@ -18,7 +18,6 @@
 
     NVMeSensor& operator=(const NVMeSensor& other) = delete;
 
-    size_t errorCount;
     int bus;
 
   private:
diff --git a/include/sensor.hpp b/include/sensor.hpp
index 21878f3..982365a 100644
--- a/include/sensor.hpp
+++ b/include/sensor.hpp
@@ -1,6 +1,7 @@
 #pragma once
 
 #include "Thresholds.hpp"
+#include "Utils.hpp"
 
 #include <sdbusplus/asio/object_server.hpp>
 
@@ -14,17 +15,23 @@
 constexpr const char* sensorValueInterface = "xyz.openbmc_project.Sensor.Value";
 constexpr const char* availableInterfaceName =
     "xyz.openbmc_project.State.Decorator.Availability";
+constexpr const char* operationalInterfaceName =
+    "xyz.openbmc_project.State.Decorator.OperationalStatus";
+constexpr const size_t errorThreshold = 5;
+
 struct Sensor
 {
     Sensor(const std::string& name,
            std::vector<thresholds::Threshold>&& thresholdData,
            const std::string& configurationPath, const std::string& objectType,
-           const double max, const double min) :
+           const double max, const double min,
+           PowerState readState = PowerState::always) :
         name(std::regex_replace(name, std::regex("[^a-zA-Z0-9_/]+"), "_")),
         configurationPath(configurationPath), objectType(objectType),
         maxValue(max), minValue(min), thresholds(std::move(thresholdData)),
         hysteresisTrigger((max - min) * 0.01),
-        hysteresisPublish((max - min) * 0.0001)
+        hysteresisPublish((max - min) * 0.0001), readState(readState),
+        errCount(0)
     {}
     virtual ~Sensor() = default;
     virtual void checkThresholds(void) = 0;
@@ -39,12 +46,14 @@
     std::shared_ptr<sdbusplus::asio::dbus_interface> thresholdInterfaceCritical;
     std::shared_ptr<sdbusplus::asio::dbus_interface> association;
     std::shared_ptr<sdbusplus::asio::dbus_interface> availableInterface;
+    std::shared_ptr<sdbusplus::asio::dbus_interface> operationalInterface;
     double value = std::numeric_limits<double>::quiet_NaN();
     bool overriddenState = false;
     bool internalSet = false;
-    bool available = true;
     double hysteresisTrigger;
     double hysteresisPublish;
+    PowerState readState;
+    size_t errCount;
 
     int setSensorValue(const double& newValue, double& oldValue)
     {
@@ -68,6 +77,11 @@
                              const std::string label = std::string(),
                              size_t thresholdSize = 0)
     {
+        if (readState == PowerState::on || readState == PowerState::biosPost)
+        {
+            setupPowerMatch(conn);
+        }
+
         createAssociation(association, configurationPath);
 
         sensorInterface->register_property("MaxValue", maxValue);
@@ -172,26 +186,101 @@
                     {
                         return 1;
                     }
+                    old = propIn;
                     if (!propIn)
                     {
                         updateValue(std::numeric_limits<double>::quiet_NaN());
                     }
-                    old = propIn;
-                    available = propIn;
                     return 1;
                 });
             availableInterface->initialize();
         }
+        if (!operationalInterface)
+        {
+            operationalInterface =
+                std::make_shared<sdbusplus::asio::dbus_interface>(
+                    conn, sensorInterface->get_object_path(),
+                    operationalInterfaceName);
+            operationalInterface->register_property("Functional", true);
+            operationalInterface->initialize();
+        }
+    }
+
+    bool readingStateGood()
+    {
+        if (readState == PowerState::on && !isPowerOn())
+        {
+            return false;
+        }
+        if (readState == PowerState::biosPost &&
+            (!hasBiosPost() || !isPowerOn()))
+        {
+            return false;
+        }
+
+        return true;
+    }
+
+    void markFunctional(bool isFunctional)
+    {
+        if (operationalInterface)
+        {
+            operationalInterface->set_property("Functional", isFunctional);
+        }
+        if (isFunctional)
+        {
+            errCount = 0;
+        }
+        else
+        {
+            updateValue(std::numeric_limits<double>::quiet_NaN());
+        }
+    }
+
+    void markAvailable(bool isAvailable)
+    {
+        if (availableInterface)
+        {
+            availableInterface->set_property("Available", isAvailable);
+            errCount = 0;
+        }
+    }
+
+    void incrementError()
+    {
+        if (!readingStateGood())
+        {
+            markAvailable(false);
+            return;
+        }
+
+        if (errCount >= errorThreshold)
+        {
+            return;
+        }
+
+        errCount++;
+        if (errCount == errorThreshold)
+        {
+            std::cerr << "Sensor " << name << " reading error!\n";
+            markFunctional(false);
+        }
     }
 
     void updateValue(const double& newValue)
     {
         // Ignore if overriding is enabled
-        if (overriddenState || !available)
+        if (overriddenState)
         {
             return;
         }
 
+        if (!readingStateGood())
+        {
+            markAvailable(false);
+            return;
+        }
+
         // Indicate that it is internal set call
         internalSet = true;
         updateProperty(sensorInterface, value, newValue, "Value");
@@ -203,6 +292,11 @@
         // which is called by checkThresholds() below,
         // in all current implementations of sensors that have thresholds.
         checkThresholds();
+        if (!std::isnan(newValue))
+        {
+            markFunctional(true);
+            markAvailable(true);
+        }
     }
 
     void updateProperty(