From 77803d9dde846804061bd7815f42768e4fe8e48e Mon Sep 17 00:00:00 2001
From: wangjue <jue.wang2@luxshare-ict.com>
Date: Tue, 24 Sep 2024 20:28:30 +0800
Subject: [PATCH] Add Intel CPU sensors

Signed-off-by: wangjue <jue.wang2@luxshare-ict.com>
---
 src/IntelCPUSensor.hpp     | 96 ++++++++++++++++++++++++++++++++++++++
 src/IntelCPUSensorMain.cpp | 26 ++++++++---
 2 files changed, 116 insertions(+), 6 deletions(-)

diff --git a/src/IntelCPUSensor.hpp b/src/IntelCPUSensor.hpp
index 0417c6e..ae90ab7 100644
--- a/src/IntelCPUSensor.hpp
+++ b/src/IntelCPUSensor.hpp
@@ -17,6 +17,12 @@
 #include <variant>
 #include <vector>
 
+extern "C"
+{
+#include <i2c/smbus.h>
+#include <linux/i2c-dev.h>
+}
+
 class IntelCPUSensor :
     public Sensor,
     public std::enable_shared_from_this<IntelCPUSensor>
@@ -114,3 +120,93 @@ inline bool cpuIsPresent(const SensorBaseConfigMap& gpioConfig)
 
     return resp;
 }
+
+inline int i2cWriteRead(std::string& i2cDev, const uint8_t slaveAddr,
+                        std::vector<uint8_t> writeData,
+                        std::vector<uint8_t>& readBuf)
+{
+    int fd = open(i2cDev.c_str(), O_RDWR | O_CLOEXEC);
+    if (fd < 0)
+    {
+        std::cerr << "unable to open i2c device \n";
+        return -1;
+    }
+
+    const size_t writeCount = writeData.size();
+    const size_t readCount = readBuf.size();
+
+    int msgCount = 0;
+    struct i2c_msg i2cmsg[2];
+
+    memset(i2cmsg, 0, sizeof(i2cmsg));
+
+    if (writeCount)
+    {
+        // Data will be writtern to the slave address
+        i2cmsg[msgCount].addr = slaveAddr;
+        i2cmsg[msgCount].flags = 0x00;
+        i2cmsg[msgCount].len = writeCount;
+        i2cmsg[msgCount].buf = writeData.data();
+        msgCount++;
+    }
+
+    if (readCount)
+    {
+        // Data will be read into the buffer from the slave address
+        i2cmsg[msgCount].addr = slaveAddr;
+        i2cmsg[msgCount].flags = I2C_M_RD;
+        i2cmsg[msgCount].len = readCount;
+        i2cmsg[msgCount].buf = readBuf.data();
+        msgCount++;
+    }
+
+    struct i2c_rdwr_ioctl_data msgReadWrite;
+    memset((void*)&msgReadWrite, 0, sizeof(msgReadWrite));
+    msgReadWrite.msgs = i2cmsg;
+    msgReadWrite.nmsgs = msgCount;
+
+    int ret = ioctl(fd, I2C_RDWR, &msgReadWrite);
+    close(fd);
+    if (ret < 0)
+    {
+        std::cerr << "I2C Read Write Failed. \n";
+        return -1;
+    }
+
+    if (readCount)
+    {
+        readBuf.resize(msgReadWrite.msgs[msgCount - 1].len);
+    }
+
+    return 0;
+}
+
+inline bool cpuIsPresentCPLD(int cpuId)
+{
+    int ret = 0;
+    uint8_t index;
+    std::string i2cDev = "/dev/i2c-0";
+    // Perform the combined write/read
+
+    std::vector<uint8_t> readBuf(1);
+    std::vector<uint8_t> writeData = {0x91, 0x02};
+    const uint8_t slaveAddr = 0x0D;
+    uint8_t regVal;
+
+    ret = i2cWriteRead(i2cDev, slaveAddr, writeData, readBuf);
+    if (ret < 0)
+    {
+        std::cerr << "i2c failed to read regVal. \n";
+        return false;
+    }
+
+    regVal = readBuf[0];
+    if(cpuId == 1)
+        index = 1;
+    else if(cpuId == 2)
+        index = 0;
+    else
+        return false;
+
+    return (regVal & (1 << index)) == 0;
+}
diff --git a/src/IntelCPUSensorMain.cpp b/src/IntelCPUSensorMain.cpp
index 0e0083a..a955c6f 100644
--- a/src/IntelCPUSensorMain.cpp
+++ b/src/IntelCPUSensorMain.cpp
@@ -721,14 +721,28 @@ bool getCpuConfig(const std::shared_ptr<sdbusplus::asio::connection>& systemBus,
                 std::string name = std::regex_replace(nameRaw, illegalDbusRegex,
                                                       "_");
 
+                int cpuId = -1;
+                auto findCpuId = cfg.find("CpuID");
+                if (findCpuId != cfg.end())
+                {
+                    cpuId = std::visit(VariantToUnsignedIntVisitor(),
+                                    findCpuId->second);
+                }
+
                 auto present = std::optional<bool>();
-                // if we can't detect it via gpio, we set presence later
-                for (const auto& [suppIntf, suppCfg] : cfgData)
+                auto findPresence = cfg.find("Presence");
+                if (findPresence != cfg.end())
                 {
-                    if (suppIntf.find("PresenceGpio") != std::string::npos)
-                    {
-                        present = cpuIsPresent(suppCfg);
-                        break;
+                    std::string method = std::visit(VariantToStringVisitor(),
+                                                 findPresence->second);
+                    if(method == "GPIO") {
+                        for (const auto& [suppIntf, suppCfg] : cfgData)
+                            present = cpuIsPresent(suppCfg);
+                    } else if(method == "CPLD") {
+                        if(cpuId != -1)
+                            present = cpuIsPresentCPLD(cpuId);
+                    } else {
+                        std::cerr << "Invalid Presence value \n";
                     }
                 }
 
-- 
2.34.1

