BPMP ABORT!

Source/hoc-clk/sysmodule/src/tsensor/aotag.cpp

@@ -22,7 +22,6 @@
 
 #include <notification.h>
 
-#include "../bpmp/bpmp.hpp"
 #include "../file/file_utils.hpp"
 #include "../mapping/mem_map.hpp"
 #include "aotag.hpp"
@@ -215,7 +214,24 @@ namespace tsensor {
             return -126;
         }
 
-        return bpmp::GetSharedInfo()->tempAO;
+        u32 regval = 0, abs = 0, fraction = 0, valid = 0, sign = 0;
+        s32 temp = 0;
+        regval = ReadPmcReg(PMC_TSENSOR_STATUS1);
+        valid = REG_GET(regval, STATUS1_TEMP_VALID);
+
+        if (!valid) {
+            return -125;
+        }
+
+        abs = REG_GET(regval, STATUS1_TEMP_ABS);
+        fraction = REG_GET(regval, STATUS1_TEMP_FRAC);
+        sign = REG_GET(regval, STATUS1_TEMP_SIGN);
+        temp = (abs * 1000) + (fraction * 500);
+        if (sign) {
+            temp = (-1) * (temp);
+        }
+
+        return temp;
     }
 
     bool IsInitialized() {

Source/hoc-clk/sysmodule/src/tsensor/soctherm.cpp

@@ -24,7 +24,6 @@
 #include <switch.h>
 
 #include "../board/board.hpp"
-#include "../bpmp/bpmp.hpp"
 #include "../file/file_utils.hpp"
 #include "../mapping/mem_map.hpp"
 #include "soctherm.hpp"
@@ -414,6 +413,15 @@ namespace tsensor {
         bool isMariko;
     }  // namespace
 
+    bool IsDisabledThroughSleep() {
+        return (ReadReg(carVa, CLK_RST_CONTROLLER_RST_DEVICES) & SWR_SOC_THERM_RST) ||
+               !(ReadReg(carVa, CLK_RST_CONTROLLER_CLK_OUT_ENB) & CLK_ENB_SOC_THERM);
+    }
+
+    bool IsSensorEnabled() {
+        return ReadReg(socthermVa, TSENSOR_TSENSOR_CLKEN);
+    }
+
     void EnableSensor(const TSensor *sensor, u32 sensorIdx) {
         u32 val = sensor->config->tall << SENSOR_CONFIG0_TALL_SHIFT;
         WriteReg(socthermVa, sensor->base + SENSOR_CONFIG0, val);
@@ -426,6 +434,21 @@ namespace tsensor {
         WriteReg(socthermVa, sensor->base + SENSOR_CONFIG2, calib[sensorIdx]);
     }
 
+    s32 TranslateTemp(u16 val) {
+        s32 t;
+
+        t = ((val & READBACK_VALUE_MASK) >> READBACK_VALUE_SHIFT) * 1000;
+        if (val & READBACK_ADD_HALF) {
+            t += 500;
+        }
+
+        if (val & READBACK_NEGATE) {
+            t *= -1;
+        }
+
+        return t;
+    }
+
     void StartSensors() {
         u32 pdiv, hotspot;
 
@@ -451,15 +474,23 @@ namespace tsensor {
     }
 
     void ReadTSensors(TSensorTemps &temps) {
-        const HocClkBpmpSharedInfo *bpmpInfo = bpmp::GetSharedInfo();
-        if (bpmpInfo->magic != HOCCLK_BPMP_MAGIC) {
+        if (IsDisabledThroughSleep()) {
             return;
         }
 
-        temps.cpu = bpmpInfo->tempCpu;
-        temps.gpu = bpmpInfo->tempGpu;
-        temps.pllx = bpmpInfo->tempPllx;
-        temps.mem = (board::GetSocType() == HocClkSocType_Erista) ? bpmpInfo->tempMem : bpmpInfo->tempPllx;
+        if (!IsSensorEnabled()) {
+            StartSensors();
+        }
+
+        temps.cpu = TranslateTemp(ReadReg(socthermVa, SENSOR_TEMP1) >> 16);
+        temps.gpu = TranslateTemp(ReadReg(socthermVa, SENSOR_TEMP1) & SENSOR_TEMP1_GPU_TEMP_MASK);
+        temps.pllx = TranslateTemp(ReadReg(socthermVa, SENSOR_TEMP2) & SENSOR_TEMP2_PLLX_TEMP_MASK);
+
+        if (board::GetSocType() == HocClkSocType_Erista) {
+            temps.mem = TranslateTemp(ReadReg(socthermVa, SENSOR_TEMP2) >> 16);
+        } else {
+            temps.mem = -1;
+        }
     }
 
     void InitializeSoctherm() {