sensord: add helper function verify_chip_id to I2CSensor (#25830)

selfdrive/sensord/sensors/bmx055_accel.cc

@@ -9,20 +9,8 @@
 BMX055_Accel::BMX055_Accel(I2CBus *bus) : I2CSensor(bus) {}
 
 int BMX055_Accel::init() {
-  int ret = 0;
-  uint8_t buffer[1];
-
-  ret = read_register(BMX055_ACCEL_I2C_REG_ID, buffer, 1);
-  if(ret < 0) {
-    LOGE("Reading chip ID failed: %d", ret);
-    goto fail;
-  }
-
-  if(buffer[0] != BMX055_ACCEL_CHIP_ID) {
-    LOGE("Chip ID wrong. Got: %d, Expected %d", buffer[0], BMX055_ACCEL_CHIP_ID);
-    ret = -1;
-    goto fail;
-  }
+  int ret = verify_chip_id(BMX055_ACCEL_I2C_REG_ID, {BMX055_ACCEL_CHIP_ID});
+  if (ret == -1) return -1;
 
   ret = set_register(BMX055_ACCEL_I2C_REG_PMU, BMX055_ACCEL_NORMAL_MODE);
   if (ret < 0) {

selfdrive/sensord/sensors/bmx055_gyro.cc

@@ -12,20 +12,8 @@
 BMX055_Gyro::BMX055_Gyro(I2CBus *bus) : I2CSensor(bus) {}
 
 int BMX055_Gyro::init() {
-  int ret = 0;
-  uint8_t buffer[1];
-
-  ret =read_register(BMX055_GYRO_I2C_REG_ID, buffer, 1);
-  if(ret < 0) {
-    LOGE("Reading chip ID failed: %d", ret);
-    goto fail;
-  }
-
-  if(buffer[0] != BMX055_GYRO_CHIP_ID) {
-    LOGE("Chip ID wrong. Got: %d, Expected %d", buffer[0], BMX055_GYRO_CHIP_ID);
-    ret = -1;
-    goto fail;
-  }
+  int ret = verify_chip_id(BMX055_GYRO_I2C_REG_ID, {BMX055_GYRO_CHIP_ID});
+  if (ret == -1) return -1;
 
   ret = set_register(BMX055_GYRO_I2C_REG_LPM1, BMX055_GYRO_NORMAL_MODE);
   if (ret < 0) {

selfdrive/sensord/sensors/bmx055_magn.cc

@@ -66,8 +66,6 @@ static int16_t compensate_z(trim_data_t trim_data, int16_t mag_data_z, uint16_t
 BMX055_Magn::BMX055_Magn(I2CBus *bus) : I2CSensor(bus) {}
 
 int BMX055_Magn::init() {
-  int ret;
-  uint8_t buffer[1];
   uint8_t trim_x1y1[2] = {0};
   uint8_t trim_x2y2[2] = {0};
   uint8_t trim_xy1xy2[2] = {0};
@@ -78,25 +76,18 @@ int BMX055_Magn::init() {
   uint8_t trim_xyz1[2] = {0};
 
   // suspend -> sleep
-  ret = set_register(BMX055_MAGN_I2C_REG_PWR_0, 0x01);
+  int ret = set_register(BMX055_MAGN_I2C_REG_PWR_0, 0x01);
   if(ret < 0) {
     LOGE("Enabling power failed: %d", ret);
     goto fail;
   }
   util::sleep_for(5); // wait until the chip is powered on
 
-  // read chip ID
-  ret = read_register(BMX055_MAGN_I2C_REG_ID, buffer, 1);
-  if(ret < 0) {
-    LOGE("Reading chip ID failed: %d", ret);
+  ret = verify_chip_id(BMX055_MAGN_I2C_REG_ID, {BMX055_MAGN_CHIP_ID});
+  if (ret == -1) {
     goto fail;
   }
 
-  if(buffer[0] != BMX055_MAGN_CHIP_ID) {
-    LOGE("Chip ID wrong. Got: %d, Expected %d", buffer[0], BMX055_MAGN_CHIP_ID);
-    return -1;
-  }
-
   // Load magnetometer trim
   ret = read_register(BMX055_MAGN_I2C_REG_DIG_X1, trim_x1y1, 2);
   if(ret < 0) goto fail;

selfdrive/sensord/sensors/bmx055_temp.cc

@@ -9,23 +9,7 @@
 BMX055_Temp::BMX055_Temp(I2CBus *bus) : I2CSensor(bus) {}
 
 int BMX055_Temp::init() {
-  int ret = 0;
-  uint8_t buffer[1];
-
-  ret = read_register(BMX055_ACCEL_I2C_REG_ID, buffer, 1);
-  if(ret < 0) {
-    LOGE("Reading chip ID failed: %d", ret);
-    goto fail;
-  }
-
-  if(buffer[0] != BMX055_ACCEL_CHIP_ID) {
-    LOGE("Chip ID wrong. Got: %d, Expected %d", buffer[0], BMX055_ACCEL_CHIP_ID);
-    ret = -1;
-    goto fail;
-  }
-
-fail:
-  return ret;
+  return verify_chip_id(BMX055_ACCEL_I2C_REG_ID, {BMX055_ACCEL_CHIP_ID}) == -1 ? -1 : 0;
 }
 
 bool BMX055_Temp::get_event(MessageBuilder &msg, uint64_t ts) {

selfdrive/sensord/sensors/i2c_sensor.h

@@ -2,12 +2,12 @@
 
 #include <cstdint>
 #include <unistd.h>
-
 #include "cereal/gen/cpp/log.capnp.h"
 
 #include "common/i2c.h"
 #include "common/gpio.h"
 
+#include "common/swaglog.h"
 #include "selfdrive/sensord/sensors/constants.h"
 #include "selfdrive/sensord/sensors/sensor.h"
 
@@ -33,4 +33,18 @@ public:
   virtual int init() = 0;
   virtual bool get_event(MessageBuilder &msg, uint64_t ts = 0) = 0;
   virtual int shutdown() = 0;
+
+  int verify_chip_id(uint8_t address, const std::vector<uint8_t> &expected_ids) {
+    uint8_t chip_id = 0;
+    int ret = read_register(address, &chip_id, 1);
+    if (ret < 0) {
+      LOGE("Reading chip ID failed: %d", ret);
+      return -1;
+    }
+    for (int i = 0; i < expected_ids.size(); ++i) {
+      if (chip_id == expected_ids[i]) return chip_id;
+    }
+    LOGE("Chip ID wrong. Got: %d, Expected %d", chip_id, expected_ids[0]);
+    return -1;
+  }
 };

selfdrive/sensord/sensors/lsm6ds3_accel.cc

@@ -118,8 +118,6 @@ int LSM6DS3_Accel::self_test(int test_type) {
 }
 
 int LSM6DS3_Accel::init() {
-  int ret = 0;
-  uint8_t buffer[1];
   uint8_t value = 0;
   bool do_self_test = false;
 
@@ -128,19 +126,10 @@ int LSM6DS3_Accel::init() {
     do_self_test = true;
   }
 
-  ret = read_register(LSM6DS3_ACCEL_I2C_REG_ID, buffer, 1);
-  if(ret < 0) {
-    LOGE("Reading chip ID failed: %d", ret);
-    goto fail;
-  }
-
-  if(buffer[0] != LSM6DS3_ACCEL_CHIP_ID && buffer[0] != LSM6DS3TRC_ACCEL_CHIP_ID) {
-    LOGE("Chip ID wrong. Got: %d, Expected %d", buffer[0], LSM6DS3_ACCEL_CHIP_ID);
-    ret = -1;
-    goto fail;
-  }
+  int ret = verify_chip_id(LSM6DS3_ACCEL_I2C_REG_ID, {LSM6DS3_ACCEL_CHIP_ID, LSM6DS3TRC_ACCEL_CHIP_ID});
+  if (ret == -1) return -1;
 
-  if (buffer[0] == LSM6DS3TRC_ACCEL_CHIP_ID) {
+  if (ret == LSM6DS3TRC_ACCEL_CHIP_ID) {
     source = cereal::SensorEventData::SensorSource::LSM6DS3TRC;
   }
 

selfdrive/sensord/sensors/lsm6ds3_gyro.cc

@@ -107,8 +107,6 @@ int LSM6DS3_Gyro::self_test(int test_type) {
 }
 
 int LSM6DS3_Gyro::init() {
-  int ret = 0;
-  uint8_t buffer[1];
   uint8_t value = 0;
   bool do_self_test = false;
 
@@ -117,19 +115,10 @@ int LSM6DS3_Gyro::init() {
     do_self_test = true;
   }
 
-  ret = read_register(LSM6DS3_GYRO_I2C_REG_ID, buffer, 1);
-  if(ret < 0) {
-    LOGE("Reading chip ID failed: %d", ret);
-    goto fail;
-  }
-
-  if(buffer[0] != LSM6DS3_GYRO_CHIP_ID && buffer[0] != LSM6DS3TRC_GYRO_CHIP_ID) {
-    LOGE("Chip ID wrong. Got: %d, Expected %d", buffer[0], LSM6DS3_GYRO_CHIP_ID);
-    ret = -1;
-    goto fail;
-  }
+  int ret = verify_chip_id(LSM6DS3_GYRO_I2C_REG_ID, {LSM6DS3_GYRO_CHIP_ID, LSM6DS3TRC_GYRO_CHIP_ID});
+  if (ret == -1) return -1;
 
-  if (buffer[0] == LSM6DS3TRC_GYRO_CHIP_ID) {
+  if (ret == LSM6DS3TRC_GYRO_CHIP_ID) {
     source = cereal::SensorEventData::SensorSource::LSM6DS3TRC;
   }
 

selfdrive/sensord/sensors/lsm6ds3_temp.cc

@@ -8,27 +8,13 @@
 LSM6DS3_Temp::LSM6DS3_Temp(I2CBus *bus) : I2CSensor(bus) {}
 
 int LSM6DS3_Temp::init() {
-  int ret = 0;
-  uint8_t buffer[1];
+  int ret = verify_chip_id(LSM6DS3_TEMP_I2C_REG_ID, {LSM6DS3_TEMP_CHIP_ID, LSM6DS3TRC_TEMP_CHIP_ID});
+  if (ret == -1) return -1;
 
-  ret = read_register(LSM6DS3_TEMP_I2C_REG_ID, buffer, 1);
-  if(ret < 0) {
-    LOGE("Reading chip ID failed: %d", ret);
-    goto fail;
-  }
-
-  if(buffer[0] != LSM6DS3_TEMP_CHIP_ID && buffer[0] != LSM6DS3TRC_TEMP_CHIP_ID) {
-    LOGE("Chip ID wrong. Got: %d, Expected %d", buffer[0], LSM6DS3_TEMP_CHIP_ID);
-    ret = -1;
-    goto fail;
-  }
-
-  if (buffer[0] == LSM6DS3TRC_TEMP_CHIP_ID) {
+  if (ret == LSM6DS3TRC_TEMP_CHIP_ID) {
     source = cereal::SensorEventData::SensorSource::LSM6DS3TRC;
   }
-
-fail:
-  return ret;
+  return 0;
 }
 
 bool LSM6DS3_Temp::get_event(MessageBuilder &msg, uint64_t ts) {

selfdrive/sensord/sensors/mmc5603nj_magn.cc

@@ -8,20 +8,8 @@
 MMC5603NJ_Magn::MMC5603NJ_Magn(I2CBus *bus) : I2CSensor(bus) {}
 
 int MMC5603NJ_Magn::init() {
-  int ret = 0;
-  uint8_t buffer[1];
-
-  ret = read_register(MMC5603NJ_I2C_REG_ID, buffer, 1);
-  if(ret < 0) {
-    LOGE("Reading chip ID failed: %d", ret);
-    goto fail;
-  }
-
-  if(buffer[0] != MMC5603NJ_CHIP_ID) {
-    LOGE("Chip ID wrong. Got: %d, Expected %d", buffer[0], MMC5603NJ_CHIP_ID);
-    ret = -1;
-    goto fail;
-  }
+  int ret = verify_chip_id(MMC5603NJ_I2C_REG_ID, {MMC5603NJ_CHIP_ID});
+  if (ret == -1) return -1;
 
   // Set 100 Hz
   ret = set_register(MMC5603NJ_I2C_REG_ODR, 100);