diff options
| -rw-r--r-- | drivers/iio/imu/inv_mpu6050/inv_mpu_core.c | 33 |
1 files changed, 24 insertions, 9 deletions
diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c b/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c index 60732e9..eea4576 100644 --- a/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c +++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c @@ -777,27 +777,42 @@ static int inv_check_and_setup_chip(struct inv_mpu6050_state *st) { int result; unsigned int regval; + int i; st->hw = &hw_info[st->chip_type]; st->reg = hw_info[st->chip_type].reg; - /* reset to make sure previous state are not there */ - result = regmap_write(st->map, st->reg->pwr_mgmt_1, - INV_MPU6050_BIT_H_RESET); - if (result) - return result; - msleep(INV_MPU6050_POWER_UP_TIME); - /* check chip self-identification */ result = regmap_read(st->map, INV_MPU6050_REG_WHOAMI, ®val); if (result) return result; if (regval != st->hw->whoami) { - dev_warn(regmap_get_device(st->map), - "whoami mismatch got %#02x expected %#02hhx for %s\n", + /* check whoami against all possible values */ + for (i = 0; i < INV_NUM_PARTS; ++i) { + if (regval == hw_info[i].whoami) { + dev_warn(regmap_get_device(st->map), + "whoami mismatch got %#02x (%s)" + "expected %#02hhx (%s)\n", + regval, hw_info[i].name, + st->hw->whoami, st->hw->name); + break; + } + } + if (i >= INV_NUM_PARTS) { + dev_err(regmap_get_device(st->map), + "invalid whoami %#02x expected %#02hhx (%s)\n", regval, st->hw->whoami, st->hw->name); + return -ENODEV; + } } + /* reset to make sure previous state are not there */ + result = regmap_write(st->map, st->reg->pwr_mgmt_1, + INV_MPU6050_BIT_H_RESET); + if (result) + return result; + msleep(INV_MPU6050_POWER_UP_TIME); + /* * toggle power state. After reset, the sleep bit could be on * or off depending on the OTP settings. Toggling power would |
