diff options
| -rw-r--r-- | driver/accel_lis2dh.c | 12 | ||||
| -rw-r--r-- | driver/accelgyro_bmi160.c | 37 | ||||
| -rw-r--r-- | driver/accelgyro_lsm6dsm.c | 4 |
3 files changed, 21 insertions, 32 deletions
diff --git a/driver/accel_lis2dh.c b/driver/accel_lis2dh.c index f7d897757b..8975769788 100644 --- a/driver/accel_lis2dh.c +++ b/driver/accel_lis2dh.c @@ -125,14 +125,10 @@ static int set_data_rate(const struct motion_sensor_t *s, int rate, int rnd) normalized_rate = LIS2DH_REG_TO_NORMALIZE(reg_val); } - /* Adjust rounded value */ - if (reg_val > LIS2DH_ODR_400HZ_VAL) { - reg_val = LIS2DH_ODR_400HZ_VAL; - normalized_rate = LIS2DH_ODR_MAX_VAL; - } else if (reg_val < LIS2DH_ODR_1HZ_VAL) { - reg_val = LIS2DH_ODR_1HZ_VAL; - normalized_rate = LIS2DH_ODR_MIN_VAL; - } + if (normalized_rate > MIN(LIS2DH_ODR_MAX_VAL, + CONFIG_EC_MAX_SENSOR_FREQ_MILLIHZ) || + normalized_rate < LIS2DH_ODR_MIN_VAL) + return EC_RES_INVALID_PARAM; /* * Lock accel resource to prevent another task from attempting diff --git a/driver/accelgyro_bmi160.c b/driver/accelgyro_bmi160.c index 5a28ae8b09..d8a3cb438b 100644 --- a/driver/accelgyro_bmi160.c +++ b/driver/accelgyro_bmi160.c @@ -385,41 +385,32 @@ static int set_data_rate(const struct motion_sensor_t *s, normalized_rate = BMI160_REG_TO_ODR(reg_val); if (rnd && (normalized_rate < rate)) { reg_val++; - normalized_rate *= 2; + normalized_rate = BMI160_REG_TO_ODR(reg_val); } switch (s->type) { case MOTIONSENSE_TYPE_ACCEL: - if (reg_val > BMI160_ODR_1600HZ) { - reg_val = BMI160_ODR_1600HZ; - normalized_rate = 1600000; - } else if (reg_val < BMI160_ODR_0_78HZ) { - reg_val = BMI160_ODR_0_78HZ; - normalized_rate = 780; - } + if (normalized_rate > MIN(BMI160_ACCEL_MAX_FREQ, + CONFIG_EC_MAX_SENSOR_FREQ_MILLIHZ) || + normalized_rate < BMI160_ACCEL_MIN_FREQ) + return EC_RES_INVALID_PARAM; break; case MOTIONSENSE_TYPE_GYRO: - if (reg_val > BMI160_ODR_3200HZ) { - reg_val = BMI160_ODR_3200HZ; - normalized_rate = 3200000; - } else if (reg_val < BMI160_ODR_25HZ) { - reg_val = BMI160_ODR_25HZ; - normalized_rate = 25000; - } + if (normalized_rate > MIN(BMI160_GYRO_MAX_FREQ, + CONFIG_EC_MAX_SENSOR_FREQ_MILLIHZ) || + normalized_rate < BMI160_GYRO_MIN_FREQ) + return EC_RES_INVALID_PARAM; break; +#ifdef CONFIG_MAG_BMI160_BMM150 case MOTIONSENSE_TYPE_MAG: /* We use the regular preset we can go about 100Hz */ - if (reg_val > BMI160_ODR_100HZ) { - reg_val = BMI160_ODR_100HZ; - normalized_rate = 100000; - } else if (reg_val < BMI160_ODR_0_78HZ) { - reg_val = BMI160_ODR_0_78HZ; - normalized_rate = 780; - } + if (reg_val > BMI160_ODR_100HZ || reg_val < BMI160_ODR_0_78HZ) + return EC_RES_INVALID_PARAM; break; +#endif default: - return -1; + return EC_RES_INVALID_PARAM; } /* diff --git a/driver/accelgyro_lsm6dsm.c b/driver/accelgyro_lsm6dsm.c index 237cd2cf9b..5341b5a01a 100644 --- a/driver/accelgyro_lsm6dsm.c +++ b/driver/accelgyro_lsm6dsm.c @@ -410,7 +410,9 @@ static int set_data_rate(const struct motion_sensor_t *s, int rate, int rnd) reg_val++; normalized_rate = LSM6DSM_REG_TO_ODR(reg_val); } - if (normalized_rate == 0) + if (normalized_rate < LSM6DSM_ODR_MIN_VAL || + normalized_rate > MIN(LSM6DSM_ODR_MAX_VAL, + CONFIG_EC_MAX_SENSOR_FREQ_MILLIHZ)) return EC_RES_INVALID_PARAM; } else { reg_val = 0; |