From 1932aff0d97b3da2d46b1522a1bf45675c6adcea Mon Sep 17 00:00:00 2001
From: Adeeb Shihadeh <adeebshihadeh@gmail.com>
Date: Fri, 25 Oct 2024 17:41:23 -0700
Subject: [PATCH] sensord: cleanup logging (#33875)

Co-authored-by: Comma Device <device@comma.ai>
---
 system/sensord/sensors/bmx055_accel.cc | 9 ++++++++-
 system/sensord/sensors/bmx055_gyro.cc  | 4 ++++
 system/sensord/sensors/bmx055_magn.cc  | 6 ++++--
 system/sensord/sensors/i2c_sensor.h    | 2 +-
 system/sensord/sensors/sensor.h        | 2 ++
 system/sensord/sensors_qcom2.cc        | 2 +-
 6 files changed, 20 insertions(+), 5 deletions(-)

diff --git a/system/sensord/sensors/bmx055_accel.cc b/system/sensord/sensors/bmx055_accel.cc
index bdb0113de3..bcc31e1d6c 100644
--- a/system/sensord/sensors/bmx055_accel.cc
+++ b/system/sensord/sensors/bmx055_accel.cc
@@ -10,12 +10,15 @@ BMX055_Accel::BMX055_Accel(I2CBus *bus) : I2CSensor(bus) {}
 
 int BMX055_Accel::init() {
   int ret = verify_chip_id(BMX055_ACCEL_I2C_REG_ID, {BMX055_ACCEL_CHIP_ID});
-  if (ret == -1) return -1;
+  if (ret == -1) {
+    goto fail;
+  }
 
   ret = set_register(BMX055_ACCEL_I2C_REG_PMU, BMX055_ACCEL_NORMAL_MODE);
   if (ret < 0) {
     goto fail;
   }
+
   // bmx055 accel has a 1.3ms wakeup time from deep suspend mode
   util::sleep_for(10);
 
@@ -36,11 +39,15 @@ int BMX055_Accel::init() {
     goto fail;
   }
 
+  enabled = true;
+
 fail:
   return ret;
 }
 
 int BMX055_Accel::shutdown()  {
+  if (!enabled) return 0;
+
   // enter deep suspend mode (lowest power mode)
   int ret = set_register(BMX055_ACCEL_I2C_REG_PMU, BMX055_ACCEL_DEEP_SUSPEND);
   if (ret < 0) {
diff --git a/system/sensord/sensors/bmx055_gyro.cc b/system/sensord/sensors/bmx055_gyro.cc
index 411b2f445e..0cc405f654 100644
--- a/system/sensord/sensors/bmx055_gyro.cc
+++ b/system/sensord/sensors/bmx055_gyro.cc
@@ -46,11 +46,15 @@ int BMX055_Gyro::init() {
     goto fail;
   }
 
+  enabled = true;
+
 fail:
   return ret;
 }
 
 int BMX055_Gyro::shutdown()  {
+  if (!enabled) return 0;
+
   // enter deep suspend mode (lowest power mode)
   int ret = set_register(BMX055_GYRO_I2C_REG_LPM1, BMX055_GYRO_DEEP_SUSPEND);
   if (ret < 0) {
diff --git a/system/sensord/sensors/bmx055_magn.cc b/system/sensord/sensors/bmx055_magn.cc
index 223f0337e8..b498c5fe3d 100644
--- a/system/sensord/sensors/bmx055_magn.cc
+++ b/system/sensord/sensors/bmx055_magn.cc
@@ -78,7 +78,7 @@ int BMX055_Magn::init() {
   // suspend -> sleep
   int ret = set_register(BMX055_MAGN_I2C_REG_PWR_0, 0x01);
   if (ret < 0) {
-    LOGW("Enabling power failed: %d", ret);
+    LOGD("Enabling power failed: %d", ret);
     goto fail;
   }
   util::sleep_for(5); // wait until the chip is powered on
@@ -139,7 +139,7 @@ int BMX055_Magn::init() {
     goto fail;
   }
 
-
+  enabled = true;
   return 0;
 
  fail:
@@ -147,6 +147,8 @@ int BMX055_Magn::init() {
 }
 
 int BMX055_Magn::shutdown() {
+  if (!enabled) return 0;
+
   // move to suspend mode
   int ret = set_register(BMX055_MAGN_I2C_REG_PWR_0, 0);
   if (ret < 0) {
diff --git a/system/sensord/sensors/i2c_sensor.h b/system/sensord/sensors/i2c_sensor.h
index 6082510083..e6d328ce72 100644
--- a/system/sensord/sensors/i2c_sensor.h
+++ b/system/sensord/sensors/i2c_sensor.h
@@ -39,7 +39,7 @@ public:
     uint8_t chip_id = 0;
     int ret = read_register(address, &chip_id, 1);
     if (ret < 0) {
-      LOGW("Reading chip ID failed: %d", ret);
+      LOGD("Reading chip ID failed: %d", ret);
       return -1;
     }
     for (int i = 0; i < expected_ids.size(); ++i) {
diff --git a/system/sensord/sensors/sensor.h b/system/sensord/sensors/sensor.h
index 1b0e3be0dc..ccf998d161 100644
--- a/system/sensord/sensors/sensor.h
+++ b/system/sensord/sensors/sensor.h
@@ -5,8 +5,10 @@
 class Sensor {
 public:
   int gpio_fd = -1;
+  bool enabled = false;
   uint64_t start_ts = 0;
   uint64_t init_delay = 500e6; // default dealy 500ms
+
   virtual ~Sensor() {}
   virtual int init() = 0;
   virtual bool get_event(MessageBuilder &msg, uint64_t ts = 0) = 0;
diff --git a/system/sensord/sensors_qcom2.cc b/system/sensord/sensors_qcom2.cc
index 55c8d2a448..532e28053f 100644
--- a/system/sensord/sensors_qcom2.cc
+++ b/system/sensord/sensors_qcom2.cc
@@ -39,7 +39,7 @@ void interrupt_loop(std::vector<std::tuple<Sensor *, std::string>> sensors) {
     }
   }
 
-  uint64_t offset = 0;
+  uint64_t offset = nanos_since_epoch() - nanos_since_boot();
   struct pollfd fd_list[1] = {0};
   fd_list[0].fd = fd;
   fd_list[0].events = POLLIN | POLLPRI;