motion_sense: Make change in range permanent

When AP changes range, unlike offset or ODR, it was not surviving init()
call. If the sensor is powered off in S3, at resume the range would be
back to the default.

To make it consistent with other attributes, remember range change until
EC powers down.

- remove get_range
- add current_range to store the range currently used.
  This is modifiable by the AP
- when the AP shutdown, revert current_range to default_range
- Remove const attribute for sensor structure when init and set_range is
  called.

BUG=chromium:1083791
BRANCH=none
TEST=One eve branch, check range is preserved even after 'shutdown -h 0'

Change-Id: Ia7126ac0cc9c3fef60b4464d95d6dd15e64b0fc4
Signed-off-by: Gwendal Grignou <gwendal@chromium.org>
Reviewed-on: https://chromium-review.googlesource.com/c/chromiumos/platform/ec/+/2215751
Reviewed-by: Yuval Peress <peress@chromium.org>

38 files changed, 136 insertions, 316 deletions

diff --git a/common/body_detection.c b/common/body_detection.c
index 375b4456d0..c15e1bc6bb 100644
--- a/common/body_detection.c
+++ b/common/body_detection.c
@@ -169,7 +169,6 @@ static void determine_threshold_scale(int range, int resolution, int rms_noise)
 void body_detect_reset(void)
 {
 	int odr = body_sensor->drv->get_data_rate(body_sensor);
-	int range = body_sensor->drv->get_range(body_sensor);
 	int resolution = body_sensor->drv->get_resolution(body_sensor);
 	int rms_noise = body_sensor->drv->get_rms_noise(body_sensor);
 
@@ -181,7 +180,8 @@ void body_detect_reset(void)
 	if (odr == 0)
 		return;
 	determine_window_size(odr);
-	determine_threshold_scale(range, resolution, rms_noise);
+	determine_threshold_scale(body_sensor->current_range,
+				  resolution, rms_noise);
 	/* initialize motion data and state */
 	memset(data, 0, sizeof(data));
 	history_idx = 0;
diff --git a/common/motion_lid.c b/common/motion_lid.c
index f8933a1b2b..0ab0dd9238 100644
--- a/common/motion_lid.c
+++ b/common/motion_lid.c
@@ -307,10 +307,8 @@ static int calculate_lid_angle(const intv3_t base, const intv3_t lid,
 	 * possible.
 	 */
 	for (i = X; i <= Z; i++) {
-		scaled_base[i] = base[i] *
-			accel_base->drv->get_range(accel_base);
-		scaled_lid[i] = lid[i] *
-			accel_lid->drv->get_range(accel_lid);
+		scaled_base[i] = base[i] * accel_base->current_range;
+		scaled_lid[i] = lid[i] * accel_lid->current_range;
 		if (ABS(scaled_base[i]) > MOTION_SCALING_AXIS_MAX ||
 		    ABS(scaled_lid[i]) > MOTION_SCALING_AXIS_MAX) {
 			reliable = 0;
diff --git a/common/motion_sense.c b/common/motion_sense.c
index e31ec5eddc..4b3f66cd07 100644
--- a/common/motion_sense.c
+++ b/common/motion_sense.c
@@ -339,19 +339,19 @@ static inline int motion_sense_init(struct motion_sensor_t *sensor)
  *
  * Called by init routine of each sensors when successful.
  */
-int sensor_init_done(const struct motion_sensor_t *s)
+int sensor_init_done(struct motion_sensor_t *s)
 {
 	int ret;
 
-	ret = s->drv->set_range(s, BASE_RANGE(s->default_range),
-				!!(s->default_range & ROUND_UP_FLAG));
+	ret = s->drv->set_range(s, BASE_RANGE(s->current_range),
+				!!(s->current_range & ROUND_UP_FLAG));
 	if (ret == EC_RES_SUCCESS) {
 		if (IS_ENABLED(CONFIG_CONSOLE_VERBOSE))
 			CPRINTS("%s: MS Done Init type:0x%X range:%d",
-				s->name, s->type, s->drv->get_range(s));
+				s->name, s->type, s->current_range);
 		else
 			CPRINTS("%c%d InitDone r:%d", s->name[0], s->type,
-				s->drv->get_range(s));
+				s->current_range);
 	}
 	return ret;
 }
@@ -369,9 +369,15 @@ static void motion_sense_switch_sensor_rate(void)
 	for (i = 0; i < motion_sensor_count; ++i) {
 		sensor = &motion_sensors[i];
 		if (SENSOR_ACTIVE(sensor)) {
-			/* Initialize or just back the odr previously set. */
+			/*
+			 * Initialize or just back the odr/range previously
+			 * set.
+			 */
 			if (sensor->state == SENSOR_INITIALIZED) {
 				motion_sense_set_data_rate(sensor);
+				sensor->drv->set_range(sensor,
+						       sensor->current_range,
+						       1);
 			} else {
 				ret = motion_sense_init(sensor);
 				if (ret != EC_SUCCESS)
@@ -408,6 +414,7 @@ static void motion_sense_shutdown(void)
 		/* Forget about changes made by the AP */
 		sensor->config[SENSOR_CONFIG_AP].odr = 0;
 		sensor->config[SENSOR_CONFIG_AP].ec_rate = 0;
+		sensor->current_range = sensor->default_range;
 	}
 	motion_sense_switch_sensor_rate();
 
@@ -1145,10 +1152,7 @@ static enum ec_status host_cmd_motion_sense(struct host_cmd_handler_args *args)
 			}
 		}
 
-		if (!sensor->drv->get_range)
-			return EC_RES_INVALID_COMMAND;
-
-		out->sensor_range.ret = sensor->drv->get_range(sensor);
+		out->sensor_range.ret = sensor->current_range;
 		args->response_size = sizeof(out->sensor_range);
 		break;
 
@@ -1485,8 +1489,7 @@ static int command_accelrange(int argc, char **argv)
 					   round) == EC_ERROR_INVAL)
 			return EC_ERROR_PARAM2;
 	} else {
-		ccprintf("Range for sensor %d: %d\n", id,
-			 sensor->drv->get_range(sensor));
+		ccprintf("Sensor %d range: %d\n", id, sensor->current_range);
 	}
 
 	return EC_SUCCESS;
@@ -1684,7 +1687,7 @@ static int command_display_accel_info(int argc, char **argv)
 		ccprintf("port: %d\n", motion_sensors[i].port);
 		ccprintf("addr: %d\n", I2C_STRIP_FLAGS(motion_sensors[i]
 						    .i2c_spi_addr_flags));
-		ccprintf("range: %d\n", motion_sensors[i].default_range);
+		ccprintf("range: %d\n", motion_sensors[i].current_range);
 		ccprintf("min_freq: %d\n", motion_sensors[i].min_frequency);
 		ccprintf("max_freq: %d\n", motion_sensors[i].max_frequency);
 		ccprintf("config:\n");
diff --git a/common/online_calibration.c b/common/online_calibration.c
index 5cef26fd00..f026f1adf7 100644
--- a/common/online_calibration.c
+++ b/common/online_calibration.c
@@ -61,7 +61,7 @@ static void data_int16_to_fp(const struct motion_sensor_t *s,
 			     const int16_t *data, fpv3_t out)
 {
 	int i;
-	fp_t range = INT_TO_FP(s->drv->get_range(s));
+	fp_t range = INT_TO_FP(s->current_range);
 
 	for (i = 0; i < 3; ++i) {
 		fp_t v = INT_TO_FP((int32_t)data[i]);
@@ -77,7 +77,7 @@ static void data_fp_to_int16(const struct motion_sensor_t *s, const fpv3_t data,
 			     int16_t *out)
 {
 	int i;
-	fp_t range = INT_TO_FP(s->drv->get_range(s));
+	fp_t range = INT_TO_FP(s->current_range);
 
 	for (i = 0; i < 3; ++i) {
 		int32_t iv;
diff --git a/driver/accel_bma2x2.c b/driver/accel_bma2x2.c
index 92e273cf9a..3b1cf72a8c 100644
--- a/driver/accel_bma2x2.c
+++ b/driver/accel_bma2x2.c
@@ -43,10 +43,9 @@ static inline int raw_write8(const int port, const uint16_t i2c_addr_flags,
 	return i2c_write8(port, i2c_addr_flags, reg, data);
 }
 
-static int set_range(const struct motion_sensor_t *s, int range, int rnd)
+static int set_range(struct motion_sensor_t *s, int range, int rnd)
 {
 	int ret,  range_val, reg_val, range_reg_val;
-	struct accelgyro_saved_data_t *data = s->drv_data;
 
 	range_val = BMA2x2_RANGE_TO_REG(range);
 	if ((BMA2x2_RANGE_TO_REG(range_val) < range) && rnd)
@@ -67,20 +66,13 @@ static int set_range(const struct motion_sensor_t *s, int range, int rnd)
 
 	/* If successfully written, then save the range. */
 	if (ret == EC_SUCCESS)
-		data->range = BMA2x2_REG_TO_RANGE(range_val);
+		s->current_range = BMA2x2_REG_TO_RANGE(range_val);
 
 	mutex_unlock(s->mutex);
 
 	return ret;
 }
 
-static int get_range(const struct motion_sensor_t *s)
-{
-	struct accelgyro_saved_data_t *data = s->drv_data;
-
-	return data->range;
-}
-
 static int get_resolution(const struct motion_sensor_t *s)
 {
 	return BMA2x2_RESOLUTION;
@@ -197,7 +189,7 @@ static int read(const struct motion_sensor_t *s, intv3_t v)
 	return EC_SUCCESS;
 }
 
-static int perform_calib(const struct motion_sensor_t *s, int enable)
+static int perform_calib(struct motion_sensor_t *s, int enable)
 {
 	int ret, val, status, rate, range, i;
 	timestamp_t deadline;
@@ -213,7 +205,7 @@ static int perform_calib(const struct motion_sensor_t *s, int enable)
 		return EC_ERROR_ACCESS_DENIED;
 
 	rate = get_data_rate(s);
-	range = get_range(s);
+	range = s->current_range;
 	/*
 	 * Temporary set frequency to 100Hz to get enough data in a short
 	 * period of time.
@@ -261,7 +253,7 @@ end_perform_calib:
 	return ret;
 }
 
-static int init(const struct motion_sensor_t *s)
+static int init(struct motion_sensor_t *s)
 {
 	int ret = 0, tries = 0, val, reg, reset_field;
 
@@ -319,7 +311,6 @@ const struct accelgyro_drv bma2x2_accel_drv = {
 	.init = init,
 	.read = read,
 	.set_range = set_range,
-	.get_range = get_range,
 	.get_resolution = get_resolution,
 	.set_data_rate = set_data_rate,
 	.get_data_rate = get_data_rate,
diff --git a/driver/accel_kionix.c b/driver/accel_kionix.c
index 141e645d99..476a0341d9 100644
--- a/driver/accel_kionix.c
+++ b/driver/accel_kionix.c
@@ -321,10 +321,9 @@ static int set_value(const struct motion_sensor_t *s, int reg, int val,
 	return ret;
 }
 
-static int set_range(const struct motion_sensor_t *s, int range, int rnd)
+static int set_range(struct motion_sensor_t *s, int range, int rnd)
 {
 	int ret, index, reg, range_field, range_val;
-	struct kionix_accel_data *data = s->drv_data;
 
 	/* Find index for interface pair matching the specified range. */
 	index = find_param_index(range, rnd, ranges[T(s)],
@@ -335,17 +334,10 @@ static int set_range(const struct motion_sensor_t *s, int range, int rnd)
 
 	ret = set_value(s, reg, range_val, range_field);
 	if (ret == EC_SUCCESS)
-		data->base.range = ranges[T(s)][index].val;
+		s->current_range = ranges[T(s)][index].val;
 	return ret;
 }
 
-static int get_range(const struct motion_sensor_t *s)
-{
-	struct kionix_accel_data *data = s->drv_data;
-
-	return data->base.range;
-}
-
 static int set_resolution(const struct motion_sensor_t *s, int res, int rnd)
 {
 	int ret, index, reg, res_field, res_val;
@@ -490,7 +482,7 @@ static int read(const struct motion_sensor_t *s, intv3_t v)
 {
 	uint8_t acc[6];
 	uint8_t reg;
-	int ret, i, range, resolution;
+	int ret, i, resolution;
 	struct kionix_accel_data *data = s->drv_data;
 
 	/* Read 6 bytes starting at XOUT_L. */
@@ -535,14 +527,13 @@ static int read(const struct motion_sensor_t *s, intv3_t v)
 	rotate(v, *s->rot_standard_ref, v);
 
 	/* apply offset in the device coordinates */
-	range = get_range(s);
 	for (i = X; i <= Z; i++)
-		v[i] += (data->offset[i] << 5) / range;
+		v[i] += (data->offset[i] << 5) / s->current_range;
 
 	return EC_SUCCESS;
 }
 
-static int init(const struct motion_sensor_t *s)
+static int init(struct motion_sensor_t *s)
 {
 	int ret, val, reg, reset_field;
 	uint8_t timeout;
@@ -677,7 +668,6 @@ const struct accelgyro_drv kionix_accel_drv = {
 	.init = init,
 	.read = read,
 	.set_range = set_range,
-	.get_range = get_range,
 	.set_resolution = set_resolution,
 	.get_resolution = get_resolution,
 	.set_data_rate = set_data_rate,
diff --git a/driver/accel_lis2dh.c b/driver/accel_lis2dh.c
index d7307d46e3..35d275b379 100644
--- a/driver/accel_lis2dh.c
+++ b/driver/accel_lis2dh.c
@@ -30,10 +30,9 @@
  * @range: Range
  * @rnd: Round up/down flag
  */
-static int set_range(const struct motion_sensor_t *s, int range, int rnd)
+static int set_range(struct motion_sensor_t *s, int range, int rnd)
 {
 	int err, normalized_range;
-	struct stprivate_data *data = s->drv_data;
 	int val;
 
 	val = LIS2DH_FS_TO_REG(range);
@@ -63,19 +62,12 @@ static int set_range(const struct motion_sensor_t *s, int range, int rnd)
 
 	/* Save Gain in range for speed up data path */
 	if (err == EC_SUCCESS)
-		data->base.range = normalized_range;
+		s->current_range = normalized_range;
 
 	mutex_unlock(s->mutex);
 	return EC_SUCCESS;
 }
 
-static int get_range(const struct motion_sensor_t *s)
-{
-	struct stprivate_data *data = s->drv_data;
-
-	return data->base.range;
-}
-
 static int set_data_rate(const struct motion_sensor_t *s, int rate, int rnd)
 {
 	int ret, normalized_rate;
@@ -168,7 +160,7 @@ static int read(const struct motion_sensor_t *s, intv3_t v)
 	return EC_SUCCESS;
 }
 
-static int init(const struct motion_sensor_t *s)
+static int init(struct motion_sensor_t *s)
 {
 	int ret = 0, tmp;
 	struct stprivate_data *data = s->drv_data;
@@ -252,7 +244,6 @@ const struct accelgyro_drv lis2dh_drv = {
 	.init = init,
 	.read = read,
 	.set_range = set_range,
-	.get_range = get_range,
 	.get_resolution = st_get_resolution,
 	.set_data_rate = set_data_rate,
 	.get_data_rate = st_get_data_rate,
diff --git a/driver/accel_lis2ds.c b/driver/accel_lis2ds.c
index 5c593bdcd0..0b57f27f21 100644
--- a/driver/accel_lis2ds.c
+++ b/driver/accel_lis2ds.c
@@ -177,11 +177,10 @@ __maybe_unused static int lis2ds_irq_handler(struct motion_sensor_t *s,
  * @range: Range
  * @rnd: Round up/down flag
  */
-static int set_range(const struct motion_sensor_t *s, int range, int rnd)
+static int set_range(struct motion_sensor_t *s, int range, int rnd)
 {
 	int err;
 	uint8_t reg_val;
-	struct stprivate_data *data = s->drv_data;
 	int newrange = ST_NORMALIZE_RATE(range);
 
 	/* Adjust and check rounded value */
@@ -200,19 +199,12 @@ static int set_range(const struct motion_sensor_t *s, int range, int rnd)
 				      reg_val);
 	if (err == EC_SUCCESS)
 		/* Save internally gain for speed optimization. */
-		data->base.range = newrange;
+		s->current_range = newrange;
 	mutex_unlock(s->mutex);
 
 	return EC_SUCCESS;
 }
 
-static int get_range(const struct motion_sensor_t *s)
-{
-	struct stprivate_data *data = s->drv_data;
-
-	return data->base.range;
-}
-
 static int set_data_rate(const struct motion_sensor_t *s, int rate, int rnd)
 {
 	int ret, normalized_rate = 0;
@@ -317,7 +309,7 @@ static int read(const struct motion_sensor_t *s, intv3_t v)
 	return EC_SUCCESS;
 }
 
-static int init(const struct motion_sensor_t *s)
+static int init(struct motion_sensor_t *s)
 {
 	int ret = 0, tmp;
 	struct stprivate_data *data = s->drv_data;
@@ -383,13 +375,11 @@ const struct accelgyro_drv lis2ds_drv = {
 	.init = init,
 	.read = read,
 	.set_range = set_range,
-	.get_range = get_range,
 	.get_resolution = st_get_resolution,
 	.set_data_rate = set_data_rate,
 	.get_data_rate = st_get_data_rate,
 	.set_offset = st_set_offset,
 	.get_offset = st_get_offset,
-	.perform_calib = NULL,
 #ifdef CONFIG_ACCEL_INTERRUPTS
 	.irq_handler = lis2ds_irq_handler,
 #endif /* CONFIG_ACCEL_INTERRUPTS */
diff --git a/driver/accel_lis2dw12.c b/driver/accel_lis2dw12.c
index e67489956a..dbc3dcac03 100644
--- a/driver/accel_lis2dw12.c
+++ b/driver/accel_lis2dw12.c
@@ -307,11 +307,10 @@ static int set_power_mode(const struct motion_sensor_t *s,
  * @range: Range
  * @rnd: Round up/down flag
  */
-static int set_range(const struct motion_sensor_t *s, int range, int rnd)
+static int set_range(struct motion_sensor_t *s, int range, int rnd)
 {
 	int err = EC_SUCCESS;
 	uint8_t reg_val;
-	struct stprivate_data *data = s->drv_data;
 	int newrange = range;
 
 	/* Adjust and check rounded value. */
@@ -338,7 +337,7 @@ static int set_range(const struct motion_sensor_t *s, int range, int rnd)
 	err = st_write_data_with_mask(s, LIS2DW12_FS_ADDR, LIS2DW12_FS_MASK,
 				      reg_val);
 	if (err == EC_SUCCESS)
-		data->base.range = newrange;
+		s->current_range = newrange;
 	else
 		goto unlock_rate;
 
@@ -352,13 +351,6 @@ unlock_rate:
 	return err;
 }
 
-static int get_range(const struct motion_sensor_t *s)
-{
-	struct stprivate_data *data = s->drv_data;
-
-	return data->base.range;
-}
-
 /**
  * ODR reg value from selected data rate in mHz.
  */
@@ -498,7 +490,7 @@ static int read(const struct motion_sensor_t *s, intv3_t v)
 	return EC_SUCCESS;
 }
 
-static int init(const struct motion_sensor_t *s)
+static int init(struct motion_sensor_t *s)
 {
 	int ret = 0, tmp, timeout = 0, status;
 	struct stprivate_data *data = s->drv_data;
@@ -592,7 +584,6 @@ const struct accelgyro_drv lis2dw12_drv = {
 	.init = init,
 	.read = read,
 	.set_range = set_range,
-	.get_range = get_range,
 	.get_resolution = st_get_resolution,
 	.set_data_rate = set_data_rate,
 	.get_data_rate = st_get_data_rate,
diff --git a/driver/accelgyro_bmi160.c b/driver/accelgyro_bmi160.c
index b89af57e62..bd32fe6d59 100644
--- a/driver/accelgyro_bmi160.c
+++ b/driver/accelgyro_bmi160.c
@@ -216,16 +216,15 @@ static int set_offset(const struct motion_sensor_t *s,
 	return ret;
 }
 
-static int perform_calib(const struct motion_sensor_t *s, int enable)
+static int perform_calib(struct motion_sensor_t *s, int enable)
 {
-	int ret, val, en_flag, status, rate, range;
+	int ret, val, en_flag, status, rate, range = s->current_range;
 	timestamp_t deadline, timeout;
 
 	if (!enable)
 		return EC_SUCCESS;
 
 	rate = bmi_get_data_rate(s);
-	range = bmi_get_range(s);
 	/*
 	 * Temporary set frequency to 100Hz to get enough data in a short
 	 * period of time.
@@ -569,7 +568,7 @@ static int irq_handler(struct motion_sensor_t *s,
 }
 #endif  /* CONFIG_ACCEL_INTERRUPTS */
 
-static int init(const struct motion_sensor_t *s)
+static int init(struct motion_sensor_t *s)
 {
 	int ret = 0, tmp, i;
 	struct accelgyro_saved_data_t *saved_data = BMI_GET_SAVED_DATA(s);
@@ -712,7 +711,6 @@ static int init(const struct motion_sensor_t *s)
 	 * so set data rate to 0.
 	 */
 	saved_data->odr = 0;
-	bmi_set_range(s, s->default_range, 0);
 
 	if (IS_ENABLED(CONFIG_ACCEL_INTERRUPTS) &&
 	    (s->type == MOTIONSENSE_TYPE_ACCEL))
@@ -725,7 +723,6 @@ const struct accelgyro_drv bmi160_drv = {
 	.init = init,
 	.read = bmi_read,
 	.set_range = bmi_set_range,
-	.get_range = bmi_get_range,
 	.get_resolution = bmi_get_resolution,
 	.set_data_rate = set_data_rate,
 	.get_data_rate = bmi_get_data_rate,
diff --git a/driver/accelgyro_bmi160.h b/driver/accelgyro_bmi160.h
index c0e8a7128b..9f8558e8b7 100644
--- a/driver/accelgyro_bmi160.h
+++ b/driver/accelgyro_bmi160.h
@@ -273,7 +273,7 @@
  * x = a * 1000 / range * 1953
  */
 #define BMI160_MOTION_TH(_s, _mg) \
-	 (MIN(((_mg) * 1000) / ((_s)->drv->get_range(_s) * 1953), 0xff))
+	 (MIN(((_mg) * 1000) / ((_s)->current_range * 1953), 0xff))
 #define BMI160_INT_MOTION_2    0x61
 #define BMI160_INT_MOTION_3    0x62
 #define BMI160_MOTION_NO_MOT_SEL   BIT(0)
@@ -295,7 +295,7 @@
 
 #define BMI160_INT_TAP_1       0x64
 #define BMI160_TAP_TH(_s, _mg) \
-	 (MIN(((_mg) * 1000) / ((_s)->drv->get_range(_s) * 31250), 0x1f))
+	 (MIN(((_mg) * 1000) / ((_s)->current_range * 31250), 0x1f))
 
 #define BMI160_INT_ORIENT_0				0x65
 
diff --git a/driver/accelgyro_bmi260.c b/driver/accelgyro_bmi260.c
index 56a3ab1a17..1be15b21c7 100644
--- a/driver/accelgyro_bmi260.c
+++ b/driver/accelgyro_bmi260.c
@@ -209,14 +209,14 @@ static int calibrate_offset(const struct motion_sensor_t *s,
 	return ret;
 }
 
-static int perform_calib(const struct motion_sensor_t *s, int enable)
+static int perform_calib(struct motion_sensor_t *s, int enable)
 {
 	int ret, rate;
 	int16_t temp;
 	int16_t offset[3];
 	intv3_t target = {0, 0, 0};
 	/* Get sensor range for calibration*/
-	int range = bmi_get_range(s);
+	int range = s->current_range;
 
 	if (!enable)
 		return EC_SUCCESS;
@@ -497,7 +497,7 @@ static int init_config(const struct motion_sensor_t *s)
 	return EC_SUCCESS;
 }
 
-static int init(const struct motion_sensor_t *s)
+static int init(struct motion_sensor_t *s)
 {
 	int ret = 0, tmp, i;
 	struct accelgyro_saved_data_t *saved_data = BMI_GET_SAVED_DATA(s);
@@ -532,7 +532,6 @@ static int init(const struct motion_sensor_t *s)
 	 * so set data rate to 0.
 	 */
 	saved_data->odr = 0;
-	bmi_set_range(s, s->default_range, 0);
 
 	if (IS_ENABLED(CONFIG_ACCEL_INTERRUPTS) &&
 	    (s->type == MOTIONSENSE_TYPE_ACCEL))
@@ -545,7 +544,6 @@ const struct accelgyro_drv bmi260_drv = {
 	.init = init,
 	.read = bmi_read,
 	.set_range = bmi_set_range,
-	.get_range = bmi_get_range,
 	.get_resolution = bmi_get_resolution,
 	.set_data_rate = set_data_rate,
 	.get_data_rate = bmi_get_data_rate,
diff --git a/driver/accelgyro_bmi_common.c b/driver/accelgyro_bmi_common.c
index 75447d54f8..d699c34cf0 100644
--- a/driver/accelgyro_bmi_common.c
+++ b/driver/accelgyro_bmi_common.c
@@ -536,15 +536,14 @@ int bmi_load_fifo(struct motion_sensor_t *s, uint32_t last_ts)
 	return EC_SUCCESS;
 }
 
-int bmi_set_range(const struct motion_sensor_t *s, int range, int rnd)
+int bmi_set_range(struct motion_sensor_t *s, int range, int rnd)
 {
 	int ret, range_tbl_size;
 	uint8_t reg_val, ctrl_reg;
 	const struct bmi_accel_param_pair *ranges;
-	struct accelgyro_saved_data_t *data = BMI_GET_SAVED_DATA(s);
 
 	if (s->type == MOTIONSENSE_TYPE_MAG) {
-		data->range = range;
+		s->current_range = range;
 		return EC_SUCCESS;
 	}
 
@@ -556,18 +555,11 @@ int bmi_set_range(const struct motion_sensor_t *s, int range, int rnd)
 			 ctrl_reg, reg_val);
 	/* Now that we have set the range, update the driver's value. */
 	if (ret == EC_SUCCESS)
-		data->range = bmi_get_engineering_val(reg_val, ranges,
-				range_tbl_size);
+		s->current_range = bmi_get_engineering_val(reg_val, ranges,
+							   range_tbl_size);
 	return ret;
 }
 
-int bmi_get_range(const struct motion_sensor_t *s)
-{
-	struct accelgyro_saved_data_t *data = BMI_GET_SAVED_DATA(s);
-
-	return data->range;
-}
-
 int bmi_get_data_rate(const struct motion_sensor_t *s)
 {
 	struct accelgyro_saved_data_t *data = BMI_GET_SAVED_DATA(s);
diff --git a/driver/accelgyro_bmi_common.h b/driver/accelgyro_bmi_common.h
index 33648b2539..d7906b68ee 100644
--- a/driver/accelgyro_bmi_common.h
+++ b/driver/accelgyro_bmi_common.h
@@ -267,9 +267,7 @@ int bmi_decode_header(struct motion_sensor_t *accel,
  */
 int bmi_load_fifo(struct motion_sensor_t *s, uint32_t last_ts);
 
-int bmi_set_range(const struct motion_sensor_t *s, int range, int rnd);
-
-int bmi_get_range(const struct motion_sensor_t *s);
+int bmi_set_range(struct motion_sensor_t *s, int range, int rnd);
 
 int bmi_get_data_rate(const struct motion_sensor_t *s);
 
diff --git a/driver/accelgyro_icm426xx.c b/driver/accelgyro_icm426xx.c
index b83c166084..4f231fdcac 100644
--- a/driver/accelgyro_icm426xx.c
+++ b/driver/accelgyro_icm426xx.c
@@ -501,7 +501,7 @@ static int icm426xx_set_range(const struct motion_sensor_t *s, int range,
 	ret = icm_field_update8(s, reg, ICM426XX_FS_MASK,
 				ICM426XX_FS_SEL(reg_val));
 	if (ret == EC_SUCCESS)
-		data->range = newrange;
+		s->current_range = newrange;
 
 	mutex_unlock(s->mutex);
 
@@ -661,7 +661,7 @@ static int icm426xx_set_offset(const struct motion_sensor_t *s,
 {
 	struct accelgyro_saved_data_t *data = ICM_GET_SAVED_DATA(s);
 	intv3_t v = { offset[X], offset[Y], offset[Z] };
-	int range, div1, div2;
+	int div1, div2;
 	int i;
 
 	/* unscale values and rotate back to chip frame */
@@ -670,16 +670,15 @@ static int icm426xx_set_offset(const struct motion_sensor_t *s,
 	rotate_inv(v, *s->rot_standard_ref, v);
 
 	/* convert raw data to hardware offset units */
-	range = icm_get_range(s);
 	switch (s->type) {
 	case MOTIONSENSE_TYPE_ACCEL:
 		/* hardware offset is 1/2048g by LSB */
-		div1 = range * 2048;
+		div1 = s->current_range * 2048;
 		div2 = 32768;
 		break;
 	case MOTIONSENSE_TYPE_GYRO:
 		/* hardware offset is 1/32dps by LSB */
-		div1 = range * 32;
+		div1 = s->current_range * 32;
 		div2 = 32768;
 		break;
 	default:
@@ -696,7 +695,7 @@ static int icm426xx_get_offset(const struct motion_sensor_t *s, int16_t *offset,
 {
 	struct accelgyro_saved_data_t *data = ICM_GET_SAVED_DATA(s);
 	intv3_t v;
-	int range, div1, div2;
+	int div1, div2;
 	int i, ret;
 
 	ret = icm426xx_get_hw_offset(s, v);
@@ -704,17 +703,16 @@ static int icm426xx_get_offset(const struct motion_sensor_t *s, int16_t *offset,
 		return ret;
 
 	/* transform offset to raw data */
-	range = icm_get_range(s);
 	switch (s->type) {
 	case MOTIONSENSE_TYPE_ACCEL:
 		/* hardware offset is 1/2048g by LSB */
 		div1 = 32768;
-		div2 = 2048 * range;
+		div2 = 2048 * s->current_range;
 		break;
 	case MOTIONSENSE_TYPE_GYRO:
 		/* hardware offset is 1/32dps by LSB */
 		div1 = 32768;
-		div2 = 32 * range;
+		div2 = 32 * s->current_range;
 		break;
 	default:
 		return EC_ERROR_INVAL;
@@ -954,7 +952,6 @@ const struct accelgyro_drv icm426xx_drv = {
 	.read = icm426xx_read,
 	.read_temp = icm426xx_read_temp,
 	.set_range = icm426xx_set_range,
-	.get_range = icm_get_range,
 	.get_resolution = icm_get_resolution,
 	.set_data_rate = icm426xx_set_data_rate,
 	.get_data_rate = icm_get_data_rate,
diff --git a/driver/accelgyro_icm_common.c b/driver/accelgyro_icm_common.c
index 03a736e76b..5a3f38dc91 100644
--- a/driver/accelgyro_icm_common.c
+++ b/driver/accelgyro_icm_common.c
@@ -282,13 +282,6 @@ int icm_get_resolution(const struct motion_sensor_t *s)
 	return ICM_RESOLUTION;
 }
 
-int icm_get_range(const struct motion_sensor_t *s)
-{
-	struct accelgyro_saved_data_t *data = ICM_GET_SAVED_DATA(s);
-
-	return data->range;
-}
-
 int icm_get_data_rate(const struct motion_sensor_t *s)
 {
 	struct accelgyro_saved_data_t *data = ICM_GET_SAVED_DATA(s);
diff --git a/driver/accelgyro_lsm6ds0.c b/driver/accelgyro_lsm6ds0.c
index 54ee1c77cc..beee41b815 100644
--- a/driver/accelgyro_lsm6ds0.c
+++ b/driver/accelgyro_lsm6ds0.c
@@ -166,14 +166,13 @@ static inline int raw_write8(const int port, const uint16_t i2c_addr_flags,
 	return i2c_write8(port, i2c_addr_flags, reg, data);
 }
 
-static int set_range(const struct motion_sensor_t *s,
+static int set_range(struct motion_sensor_t *s,
 				int range,
 				int rnd)
 {
 	int ret, ctrl_val, range_tbl_size;
 	uint8_t ctrl_reg, reg_val;
 	const struct accel_param_pair *ranges;
-	struct lsm6ds0_data *data = s->drv_data;
 
 	ctrl_reg = get_ctrl_reg(s->type);
 	ranges = get_range_table(s->type, &range_tbl_size);
@@ -197,21 +196,14 @@ static int set_range(const struct motion_sensor_t *s,
 
 	/* Now that we have set the range, update the driver's value. */
 	if (ret == EC_SUCCESS)
-		data->base.range = get_engineering_val(reg_val, ranges,
-				range_tbl_size);
+		s->current_range = get_engineering_val(reg_val, ranges,
+						       range_tbl_size);
 
 accel_cleanup:
 	mutex_unlock(s->mutex);
 	return ret;
 }
 
-static int get_range(const struct motion_sensor_t *s)
-{
-	struct lsm6ds0_data *data = s->drv_data;
-
-	return data->base.range;
-}
-
 static int get_resolution(const struct motion_sensor_t *s)
 {
 	return LSM6DS0_RESOLUTION;
@@ -328,7 +320,7 @@ static int read(const struct motion_sensor_t *s, intv3_t v)
 {
 	uint8_t raw[6];
 	uint8_t xyz_reg;
-	int ret, range, i, tmp = 0;
+	int ret, i, tmp = 0;
 	struct lsm6ds0_data *data = s->drv_data;
 
 	ret = is_data_ready(s, &tmp);
@@ -364,14 +356,13 @@ static int read(const struct motion_sensor_t *s, intv3_t v)
 	rotate(v, *s->rot_standard_ref, v);
 
 	/* apply offset in the device coordinates */
-	range = get_range(s);
 	for (i = X; i <= Z; i++)
-		v[i] += (data->offset[i] << 5) / range;
+		v[i] += (data->offset[i] << 5) / s->current_range;
 
 	return EC_SUCCESS;
 }
 
-static int init(const struct motion_sensor_t *s)
+static int init(struct motion_sensor_t *s)
 {
 	int ret = 0, tmp;
 
@@ -425,11 +416,9 @@ const struct accelgyro_drv lsm6ds0_drv = {
 	.init = init,
 	.read = read,
 	.set_range = set_range,
-	.get_range = get_range,
 	.get_resolution = get_resolution,
 	.set_data_rate = set_data_rate,
 	.get_data_rate = get_data_rate,
 	.set_offset = set_offset,
 	.get_offset = get_offset,
-	.perform_calib = NULL,
 };
diff --git a/driver/accelgyro_lsm6dsm.c b/driver/accelgyro_lsm6dsm.c
index 3383d36741..1a1308c3f5 100644
--- a/driver/accelgyro_lsm6dsm.c
+++ b/driver/accelgyro_lsm6dsm.c
@@ -514,11 +514,10 @@ __maybe_unused static int irq_handler(
  * @rnd: Round up/down flag
  * Note: Range is sensitivity/gain for speed purpose
  */
-static int set_range(const struct motion_sensor_t *s, int range, int rnd)
+static int set_range(struct motion_sensor_t *s, int range, int rnd)
 {
 	int err;
 	uint8_t ctrl_reg, reg_val;
-	struct stprivate_data *data = s->drv_data;
 	int newrange = range;
 
 	switch (s->type) {
@@ -551,25 +550,12 @@ static int set_range(const struct motion_sensor_t *s, int range, int rnd)
 	err = st_write_data_with_mask(s, ctrl_reg, LSM6DSM_RANGE_MASK, reg_val);
 	if (err == EC_SUCCESS)
 		/* Save internally gain for speed optimization. */
-		data->base.range = newrange;
+		s->current_range = newrange;
 	mutex_unlock(s->mutex);
 	return err;
 }
 
 /**
- * get_range - get full scale range
- * @s: Motion sensor pointer
- *
- * For mag range is fixed to LIS2MDL_RANGE by hardware
- */
-static int get_range(const struct motion_sensor_t *s)
-{
-	struct stprivate_data *data = s->drv_data;
-
-	return data->base.range;
-}
-
-/**
  * lsm6dsm_set_data_rate
  * @s: Motion sensor pointer
  * @range: Rate (mHz)
@@ -716,7 +702,7 @@ static int read(const struct motion_sensor_t *s, intv3_t v)
 	return EC_SUCCESS;
 }
 
-static int init(const struct motion_sensor_t *s)
+static int init(struct motion_sensor_t *s)
 {
 	int ret = 0, tmp;
 	struct stprivate_data *data = s->drv_data;
@@ -852,7 +838,6 @@ const struct accelgyro_drv lsm6dsm_drv = {
 	.init = init,
 	.read = read,
 	.set_range = set_range,
-	.get_range = get_range,
 	.get_resolution = st_get_resolution,
 	.set_data_rate = lsm6dsm_set_data_rate,
 	.get_data_rate = st_get_data_rate,
diff --git a/driver/accelgyro_lsm6dso.c b/driver/accelgyro_lsm6dso.c
index 7370ddff6b..64f7cad7c9 100644
--- a/driver/accelgyro_lsm6dso.c
+++ b/driver/accelgyro_lsm6dso.c
@@ -272,11 +272,10 @@ static int irq_handler(struct motion_sensor_t *s, uint32_t *event)
  * @rnd: Round up/down flag
  * Note: Range is sensitivity/gain for speed purpose
  */
-static int set_range(const struct motion_sensor_t *s, int range, int rnd)
+static int set_range(struct motion_sensor_t *s, int range, int rnd)
 {
 	int err;
 	uint8_t ctrl_reg, reg_val;
-	struct stprivate_data *data = LSM6DSO_GET_DATA(s);
 	int newrange = range;
 
 	ctrl_reg = LSM6DSO_RANGE_REG(s->type);
@@ -305,7 +304,7 @@ static int set_range(const struct motion_sensor_t *s, int range, int rnd)
 	err = st_write_data_with_mask(s, ctrl_reg, LSM6DSO_RANGE_MASK,
 				      reg_val);
 	if (err == EC_SUCCESS)
-		data->base.range = newrange;
+		s->current_range = newrange;
 
 	mutex_unlock(s->mutex);
 
@@ -313,17 +312,6 @@ static int set_range(const struct motion_sensor_t *s, int range, int rnd)
 }
 
 /**
- * get_range - get full scale range
- * @s: Motion sensor pointer
- */
-static int get_range(const struct motion_sensor_t *s)
-{
-	struct stprivate_data *data = LSM6DSO_GET_DATA(s);
-
-	return data->base.range;
-}
-
-/**
  * set_data_rate set sensor data rate
  * @s: Motion sensor pointer
  * @range: Rate (mHz)
@@ -425,7 +413,7 @@ static int read(const struct motion_sensor_t *s, intv3_t v)
 	return EC_SUCCESS;
 }
 
-static int init(const struct motion_sensor_t *s)
+static int init(struct motion_sensor_t *s)
 {
 	int ret = 0, tmp;
 	struct stprivate_data *data = LSM6DSO_GET_DATA(s);
@@ -493,7 +481,6 @@ const struct accelgyro_drv lsm6dso_drv = {
 	.init = init,
 	.read = read,
 	.set_range = set_range,
-	.get_range = get_range,
 	.get_resolution = st_get_resolution,
 	.set_data_rate = set_data_rate,
 	.get_data_rate = st_get_data_rate,
diff --git a/driver/als_bh1730.c b/driver/als_bh1730.c
index 872fdaa012..56afb86e0b 100644
--- a/driver/als_bh1730.c
+++ b/driver/als_bh1730.c
@@ -86,15 +86,15 @@ static int bh1730_read_lux(const struct motion_sensor_t *s, intv3_t v)
 		return EC_SUCCESS;
 }
 
-static int bh1730_set_range(const struct motion_sensor_t *s, int range,
+static int bh1730_set_range(struct motion_sensor_t *s, int range,
 			     int rnd)
 {
-	return EC_SUCCESS;
-}
+	/* Range is fixed by hardware */
+	if (range != s->default_range)
+		return EC_ERROR_INVAL;
 
-static int bh1730_get_range(const struct motion_sensor_t *s)
-{
-	return 1;
+	s->current_range = range;
+	return EC_SUCCESS;
 }
 
 static int bh1730_set_data_rate(const struct motion_sensor_t *s,
@@ -134,7 +134,7 @@ static int bh1730_get_offset(const struct motion_sensor_t *s,
 /**
  * Initialise BH1730 Ambient light sensor.
  */
-static int bh1730_init(const struct motion_sensor_t *s)
+static int bh1730_init(struct motion_sensor_t *s)
 {
 	int ret;
 
@@ -172,7 +172,6 @@ const struct accelgyro_drv bh1730_drv = {
 	.init = bh1730_init,
 	.read = bh1730_read_lux,
 	.set_range = bh1730_set_range,
-	.get_range = bh1730_get_range,
 	.set_offset = bh1730_set_offset,
 	.get_offset = bh1730_get_offset,
 	.set_data_rate = bh1730_set_data_rate,
diff --git a/driver/als_opt3001.c b/driver/als_opt3001.c
index 8f1e2bc775..53f2b7df89 100644
--- a/driver/als_opt3001.c
+++ b/driver/als_opt3001.c
@@ -177,23 +177,17 @@ int opt3001_read_lux(const struct motion_sensor_t *s, intv3_t v)
 	}
 }
 
-static int opt3001_set_range(const struct motion_sensor_t *s, int range,
+static int opt3001_set_range(struct motion_sensor_t *s, int range,
 			     int rnd)
 {
 	struct opt3001_drv_data_t *drv_data = OPT3001_GET_DATA(s);
 
 	drv_data->scale = range >> 16;
 	drv_data->uscale = range & 0xffff;
+	s->current_range = range;
 	return EC_SUCCESS;
 }
 
-static int opt3001_get_range(const struct motion_sensor_t *s)
-{
-	struct opt3001_drv_data_t *drv_data = OPT3001_GET_DATA(s);
-
-	return (drv_data->scale << 16) | (drv_data->uscale);
-}
-
 static int opt3001_set_data_rate(const struct motion_sensor_t *s,
 				int rate, int roundup)
 {
@@ -265,7 +259,7 @@ static int opt3001_get_offset(const struct motion_sensor_t *s,
 /**
  * Initialise OPT3001 light sensor.
  */
-static int opt3001_init(const struct motion_sensor_t *s)
+static int opt3001_init(struct motion_sensor_t *s)
 {
 	int data;
 	int ret;
@@ -301,7 +295,6 @@ const struct accelgyro_drv opt3001_drv = {
 	.init = opt3001_init,
 	.read = opt3001_read_lux,
 	.set_range = opt3001_set_range,
-	.get_range = opt3001_get_range,
 	.set_offset = opt3001_set_offset,
 	.get_offset = opt3001_get_offset,
 	.set_data_rate = opt3001_set_data_rate,
diff --git a/driver/als_si114x.c b/driver/als_si114x.c
index 35405bbea7..98b6c02dd5 100644
--- a/driver/als_si114x.c
+++ b/driver/als_si114x.c
@@ -23,7 +23,7 @@
 #define CPRINTF(format, args...) cprintf(CC_ACCEL, format, ## args)
 #define CPRINTS(format, args...) cprints(CC_ACCEL, format, ## args)
 
-static int init(const struct motion_sensor_t *s);
+static int init(struct motion_sensor_t *s);
 
 /**
  * Read 8bit register from device.
@@ -500,22 +500,17 @@ static int get_resolution(const struct motion_sensor_t *s)
 	return val & 0x07;
 }
 
-static int set_range(const struct motion_sensor_t *s,
+static int set_range(struct motion_sensor_t *s,
 				int range,
 				int rnd)
 {
 	struct si114x_typed_data_t *data = SI114X_GET_TYPED_DATA(s);
 	data->scale = range >> 16;
 	data->uscale = range & 0xffff;
+	s->current_range = range;
 	return EC_SUCCESS;
 }
 
-static int get_range(const struct motion_sensor_t *s)
-{
-	struct si114x_typed_data_t *data = SI114X_GET_TYPED_DATA(s);
-	return (data->scale << 16) | (data->uscale);
-}
-
 static int get_data_rate(const struct motion_sensor_t *s)
 {
 	/* Sensor in forced mode, rate is used by motion_sense */
@@ -553,7 +548,7 @@ static int get_offset(const struct motion_sensor_t *s,
 	return EC_SUCCESS;
 }
 
-static int init(const struct motion_sensor_t *s)
+static int init(struct motion_sensor_t *s)
 {
 	int ret, resol;
 	struct si114x_drv_data_t *data = SI114X_GET_DATA(s);
@@ -590,7 +585,6 @@ const struct accelgyro_drv si114x_drv = {
 	.init = init,
 	.read = read,
 	.set_range = set_range,
-	.get_range = get_range,
 	.set_resolution = set_resolution,
 	.get_resolution = get_resolution,
 	.set_data_rate = set_data_rate,
diff --git a/driver/als_tcs3400.c b/driver/als_tcs3400.c
index 9ffe3cfab1..cbf9aa7d19 100644
--- a/driver/als_tcs3400.c
+++ b/driver/als_tcs3400.c
@@ -600,25 +600,19 @@ static int tcs3400_rgb_set_data_rate(const struct motion_sensor_t *s,
 }
 
 /* Enable/disable special factory calibration mode */
-static int tcs3400_perform_calib(const struct motion_sensor_t *s,
-				     int enable)
+static int tcs3400_perform_calib(struct motion_sensor_t *s, int enable)
 {
 	TCS3400_RGB_DRV_DATA(s+1)->calibration_mode = enable;
 	return EC_SUCCESS;
 }
 
-static int tcs3400_get_range(const struct motion_sensor_t *s)
-{
-	return (TCS3400_DRV_DATA(s)->als_cal.scale << 16) |
-			(TCS3400_DRV_DATA(s)->als_cal.uscale);
-}
-
-static int tcs3400_set_range(const struct motion_sensor_t *s,
+static int tcs3400_set_range(struct motion_sensor_t *s,
 			     int range,
 			     int rnd)
 {
 	TCS3400_DRV_DATA(s)->als_cal.scale = range >> 16;
 	TCS3400_DRV_DATA(s)->als_cal.uscale = range & 0xffff;
+	s->current_range = range;
 	return EC_SUCCESS;
 }
 
@@ -703,12 +697,12 @@ static int tcs3400_set_data_rate(const struct motion_sensor_t *s,
 /**
  * Initialise TCS3400 light sensor.
  */
-static int tcs3400_rgb_init(const struct motion_sensor_t *s)
+static int tcs3400_rgb_init(struct motion_sensor_t *s)
 {
 	return EC_SUCCESS;
 }
 
-static int tcs3400_init(const struct motion_sensor_t *s)
+static int tcs3400_init(struct motion_sensor_t *s)
 {
 	/*
 	 * These are default power-on register values with two exceptions:
@@ -761,7 +755,6 @@ const struct accelgyro_drv tcs3400_drv = {
 	.init = tcs3400_init,
 	.read = tcs3400_read,
 	.set_range = tcs3400_set_range,
-	.get_range = tcs3400_get_range,
 	.set_offset = tcs3400_set_offset,
 	.get_offset = tcs3400_get_offset,
 	.set_scale = tcs3400_set_scale,
diff --git a/driver/baro_bmp280.c b/driver/baro_bmp280.c
index 56376c8aa1..037a77d963 100644
--- a/driver/baro_bmp280.c
+++ b/driver/baro_bmp280.c
@@ -243,7 +243,7 @@ static int bmp280_set_power_mode(const struct motion_sensor_t *s,
 			  BMP280_CTRL_MEAS_REG, val);
 }
 
-static int bmp280_set_range(const struct motion_sensor_t *s,
+static int bmp280_set_range(struct motion_sensor_t *s,
 				int range,
 				int rnd)
 {
@@ -253,16 +253,10 @@ static int bmp280_set_range(const struct motion_sensor_t *s,
 	 * measurment to fit into 16 bits (or less if the AP wants to).
 	 */
 	data->range = 15 - __builtin_clz(range);
+	s->current_range = 1 << (16 + data->range);
 	return EC_SUCCESS;
 }
 
-static int bmp280_get_range(const struct motion_sensor_t *s)
-{
-	struct bmp280_drv_data_t *data = BMP280_GET_DATA(s);
-
-	return 1 << (16 + data->range);
-}
-
 /*
  * bmp280_init() - Used to initialize barometer with default config
  *
@@ -270,7 +264,7 @@ static int bmp280_get_range(const struct motion_sensor_t *s)
  * @retval 0 -> Success
  */
 
-static int bmp280_init(const struct motion_sensor_t *s)
+static int bmp280_init(struct motion_sensor_t *s)
 {
 	int val, ret;
 
@@ -372,7 +366,6 @@ const struct accelgyro_drv bmp280_drv = {
 	.init = bmp280_init,
 	.read = bmp280_read,
 	.set_range = bmp280_set_range,
-	.get_range = bmp280_get_range,
 	.set_data_rate = bmp280_set_data_rate,
 	.get_data_rate = bmp280_get_data_rate,
 };
diff --git a/driver/gyro_l3gd20h.c b/driver/gyro_l3gd20h.c
index 4fcaf15ed0..77dd888542 100644
--- a/driver/gyro_l3gd20h.c
+++ b/driver/gyro_l3gd20h.c
@@ -134,14 +134,13 @@ static inline int raw_write8(const int port, const int addr, const int reg,
 	return i2c_write8(port, addr, reg, data);
 }
 
-static int set_range(const struct motion_sensor_t *s,
+static int set_range(struct motion_sensor_t *s,
 				int range,
 				int rnd)
 {
 	int ret, ctrl_val, range_tbl_size;
 	uint8_t ctrl_reg, reg_val;
 	const struct gyro_param_pair *ranges;
-	struct l3gd20_data *data = (struct l3gd20_data *)s->drv_data;
 
 	ctrl_reg = L3GD20_CTRL_REG4;
 	ranges = get_range_table(s->type, &range_tbl_size);
@@ -163,21 +162,14 @@ static int set_range(const struct motion_sensor_t *s,
 
 	/* Now that we have set the range, update the driver's value. */
 	if (ret == EC_SUCCESS)
-		data->base.range = get_engineering_val(reg_val, ranges,
-							 range_tbl_size);
+		s->current_range = get_engineering_val(reg_val, ranges,
+						       range_tbl_size);
 
 gyro_cleanup:
 	mutex_unlock(s->mutex);
 	return EC_SUCCESS;
 }
 
-static int get_range(const struct motion_sensor_t *s)
-{
-	struct l3gd20_data *data = (struct l3gd20_data *)s->drv_data;
-
-	return data->base.range;
-}
-
 static int get_resolution(const struct motion_sensor_t *s)
 {
 	return L3GD20_RESOLUTION;
@@ -349,14 +341,14 @@ static int read(const struct motion_sensor_t *s, intv3_t v)
 	rotate(v, *s->rot_standard_ref, v);
 
 	/* apply offset in the device coordinates */
-	range = get_range(s);
+	range = s->current_range;
 	for (i = X; i <= Z; i++)
 		v[i] += (data->offset[i] << 5) / range;
 
 	return EC_SUCCESS;
 }
 
-static int init(const struct motion_sensor_t *s)
+static int init(struct motion_sensor_t *s)
 {
 	int ret = 0, tmp;
 
@@ -392,7 +384,6 @@ const struct accelgyro_drv l3gd20h_drv = {
 	.init = init,
 	.read = read,
 	.set_range = set_range,
-	.get_range = get_range,
 	.get_resolution = get_resolution,
 	.set_data_rate = set_data_rate,
 	.get_data_rate = get_data_rate,
diff --git a/driver/mag_bmm150.c b/driver/mag_bmm150.c
index 2289a730f9..ad1eba7ad0 100644
--- a/driver/mag_bmm150.c
+++ b/driver/mag_bmm150.c
@@ -80,7 +80,7 @@
 } while (0)
 
 
-int bmm150_init(const struct motion_sensor_t *s)
+int bmm150_init(struct motion_sensor_t *s)
 {
 	int ret;
 	int val;
diff --git a/driver/mag_bmm150.h b/driver/mag_bmm150.h
index cf80950b8d..9f517f8097 100644
--- a/driver/mag_bmm150.h
+++ b/driver/mag_bmm150.h
@@ -134,7 +134,7 @@ struct bmm150_private_data {
 #endif
 
 /* Specific initialization of BMM150 when behing BMI160 */
-int bmm150_init(const struct motion_sensor_t *s);
+int bmm150_init(struct motion_sensor_t *s);
 
 /* Command to normalize and apply temperature compensation */
 void bmm150_normalize(const struct motion_sensor_t *s,
diff --git a/driver/mag_lis2mdl.c b/driver/mag_lis2mdl.c
index a9a00cc7c9..c0f7dff90d 100644
--- a/driver/mag_lis2mdl.c
+++ b/driver/mag_lis2mdl.c
@@ -66,25 +66,16 @@ void lis2mdl_normalize(const struct motion_sensor_t *s,
 	v[Z] += cal->bias[Z];
 }
 
-static int set_range(const struct motion_sensor_t *s, int range, int rnd)
+static int set_range(struct motion_sensor_t *s, int range, int rnd)
 {
-	struct stprivate_data *data = s->drv_data;
-
 	/* Range is fixed by hardware */
 	if (range != s->default_range)
 		return EC_ERROR_INVAL;
 
-	data->base.range = range;
+	s->current_range = range;
 	return EC_SUCCESS;
 }
 
-static int get_range(const struct motion_sensor_t *s)
-{
-	struct stprivate_data *data = s->drv_data;
-
-	return data->base.range;
-}
-
 /**
  * set_offset - Set data offset
  * @s: Motion sensor pointer
@@ -140,7 +131,7 @@ int lis2mdl_thru_lsm6dsm_read(const struct motion_sensor_t *s, intv3_t v)
 	return ret;
 }
 
-int lis2mdl_thru_lsm6dsm_init(const struct motion_sensor_t *s)
+int lis2mdl_thru_lsm6dsm_init(struct motion_sensor_t *s)
 {
 	int ret = EC_ERROR_UNIMPLEMENTED;
 	struct mag_cal_t *cal = LIS2MDL_CAL(s);
@@ -261,7 +252,7 @@ int lis2mdl_read(const struct motion_sensor_t *s, intv3_t v)
 /**
  * Initialize the sensor. This function will verify the who-am-I register
  */
-int lis2mdl_init(const struct motion_sensor_t *s)
+int lis2mdl_init(struct motion_sensor_t *s)
 {
 	int ret = EC_ERROR_UNKNOWN, who_am_i, count = LIS2MDL_STARTUP_MS;
 	struct stprivate_data *data = s->drv_data;
@@ -405,7 +396,6 @@ const struct accelgyro_drv lis2mdl_drv = {
 	.set_data_rate = lis2mdl_set_data_rate,
 #endif /* !CONFIG_MAG_LSM6DSM_LIS2MDL */
 	.set_range = set_range,
-	.get_range = get_range,
 	.get_data_rate = st_get_data_rate,
 	.get_resolution = st_get_resolution,
 	.set_offset = set_offset,
diff --git a/driver/stm_mems_common.c b/driver/stm_mems_common.c
index 6cee29977d..d3088521d9 100644
--- a/driver/stm_mems_common.c
+++ b/driver/stm_mems_common.c
@@ -125,7 +125,7 @@ int st_get_data_rate(const struct motion_sensor_t *s)
  */
 void st_normalize(const struct motion_sensor_t *s, intv3_t v, uint8_t *data)
 {
-	int i, range;
+	int i;
 	struct stprivate_data *drvdata = s->drv_data;
 	/*
 	 * Data is left-aligned and the bottom bits need to be
@@ -139,8 +139,6 @@ void st_normalize(const struct motion_sensor_t *s, intv3_t v, uint8_t *data)
 
 	rotate(v, *s->rot_standard_ref, v);
 
-	/* apply offset in the device coordinates */
-	range = s->drv->get_range(s);
 	for (i = X; i <= Z; i++)
-		v[i] += (drvdata->offset[i] << 5) / range;
+		v[i] += (drvdata->offset[i] << 5) / s->current_range;
 }
diff --git a/driver/sync.c b/driver/sync.c
index 91d161765a..2d412355fb 100644
--- a/driver/sync.c
+++ b/driver/sync.c
@@ -97,7 +97,7 @@ static int motion_irq_handler(struct motion_sensor_t *s, uint32_t *event)
 	return EC_SUCCESS;
 }
 
-static int sync_init(const struct motion_sensor_t *s)
+static int sync_init(struct motion_sensor_t *s)
 {
 	vector.sensor_num = s - motion_sensors;
 	sync_enabled = 0;
diff --git a/include/accelgyro.h b/include/accelgyro.h
index c85922f3ef..5d755b7eaa 100644
--- a/include/accelgyro.h
+++ b/include/accelgyro.h
@@ -22,11 +22,10 @@
 struct accelgyro_drv {
 	/**
 	 * Initialize accelerometers.
-	 * @s Pointer to sensor data pointer. Sensor data will be
-	 * allocated on success.
+	 * @s Pointer to sensor data pointer.
 	 * @return EC_SUCCESS if successful, non-zero if error.
 	 */
-	int (*init)(const struct motion_sensor_t *s);
+	int (*init)(struct motion_sensor_t *s);
 
 	/**
 	 * Read all three accelerations of an accelerometer. Note that all
@@ -48,19 +47,22 @@ struct accelgyro_drv {
 	int (*read_temp)(const struct motion_sensor_t *s, int *temp);
 
 	/**
-	 * Setter and getter methods for the sensor range. The sensor range
+	 * Setter method for the sensor range. The sensor range
 	 * defines the maximum value that can be returned from read(). As the
 	 * range increases, the resolution gets worse.
 	 * @s Pointer to sensor data.
 	 * @range Range (Units are +/- G's for accel, +/- deg/s for gyro)
 	 * @rnd Rounding flag. If true, it rounds up to nearest valid
 	 * value. Otherwise, it rounds down.
+	 *
+	 * sensor->current_range is updated.
+	 * It will be preserved unless EC reboots or AP is shutdown (S5).
+	 *
 	 * @return EC_SUCCESS if successful, non-zero if error.
 	 */
-	int (*set_range)(const struct motion_sensor_t *s,
+	int (*set_range)(struct motion_sensor_t *s,
 			int range,
 			int rnd);
-	int (*get_range)(const struct motion_sensor_t *s);
 
 	/**
 	 * Setter and getter methods for the sensor resolution.
@@ -121,7 +123,7 @@ struct accelgyro_drv {
 	 * Either a one shot mode (enable is not used),
 	 * or enter/exit a calibration state.
 	 */
-	int (*perform_calib)(const struct motion_sensor_t *s,
+	int (*perform_calib)(struct motion_sensor_t *s,
 				int        enable);
 	/**
 	 * handler for interrupts triggered by the sensor: it runs in task and
@@ -181,7 +183,6 @@ enum rgb_index {
 /* Used to save sensor information */
 struct accelgyro_saved_data_t {
 	int odr;
-	int range;
 	uint16_t scale[3];
 };
 
diff --git a/include/motion_sense.h b/include/motion_sense.h
index f46935ca73..e19107e290 100644
--- a/include/motion_sense.h
+++ b/include/motion_sense.h
@@ -186,6 +186,9 @@ struct motion_sensor_t {
 	 */
 	int default_range;
 
+	/* Range currently used by the sensor. */
+	int current_range;
+
 	/*
 	 * There are 4 configuration parameters to deal with different
 	 * configuration
@@ -282,7 +285,7 @@ extern unsigned int motion_min_interval;
  *
  * @param sensor sensor which was just initialized
  */
-int sensor_init_done(const struct motion_sensor_t *sensor);
+int sensor_init_done(struct motion_sensor_t *sensor);
 
 /**
  * Board specific function that is called when a double_tap event is detected.
diff --git a/test/body_detection.c b/test/body_detection.c
index 48fefce249..fe86b457c5 100644
--- a/test/body_detection.c
+++ b/test/body_detection.c
@@ -20,8 +20,7 @@ static const int window_size = 50; /* sensor data rate (Hz) */
 static int filler(const struct motion_sensor_t *s, const float v)
 {
 	int resolution = s->drv->get_resolution(s);
-	int range = s->drv->get_range(s);
-	int data_1g = BIT(resolution - 1) / range;
+	int data_1g = BIT(resolution - 1) / s->current_range;
 
 	return (int)(v * data_1g / 9.8);
 }
diff --git a/test/motion_angle.c b/test/motion_angle.c
index 06872a7a41..30f663de14 100644
--- a/test/motion_angle.c
+++ b/test/motion_angle.c
@@ -26,7 +26,7 @@
 /* Array units is in m/s^2 - old matrix format. */
 int filler(const struct motion_sensor_t *s, const float v)
 {
-	return (v * MOTION_SCALING_FACTOR) / s->drv->get_range(s);
+	return (v * MOTION_SCALING_FACTOR) / s->current_range;
 }
 
 static int test_lid_angle_less180(void)
diff --git a/test/motion_angle_tablet.c b/test/motion_angle_tablet.c
index 0829d9bb31..3cf496ac5b 100644
--- a/test/motion_angle_tablet.c
+++ b/test/motion_angle_tablet.c
@@ -28,7 +28,7 @@ int filler(const struct motion_sensor_t *s, const float v)
 {
 	return FP_TO_INT( fp_div(
 		FLOAT_TO_FP(v) * MOTION_SCALING_FACTOR,
-		fp_mul(INT_TO_FP(s->drv->get_range(s)), MOTION_ONE_G)));
+		fp_mul(INT_TO_FP(s->current_range), MOTION_ONE_G)));
 }
 
 static int test_lid_angle_less180(void)
diff --git a/test/motion_common.c b/test/motion_common.c
index 2cacae0eb1..4efa407b9f 100644
--- a/test/motion_common.c
+++ b/test/motion_common.c
@@ -15,9 +15,9 @@
 
 /*****************************************************************************/
 /* Mock functions */
-static int accel_init(const struct motion_sensor_t *s)
+static int accel_init(struct motion_sensor_t *s)
 {
-	return EC_SUCCESS;
+	return sensor_init_done(s);
 }
 
 static int accel_read(const struct motion_sensor_t *s, intv3_t v)
@@ -26,9 +26,10 @@ static int accel_read(const struct motion_sensor_t *s, intv3_t v)
 	return EC_SUCCESS;
 }
 
-static int accel_get_range(const struct motion_sensor_t *s)
+static int accel_set_range(struct motion_sensor_t *s, int range, int rnd)
 {
-	return s->default_range;
+	s->current_range = range;
+	return EC_SUCCESS;
 }
 
 static int accel_get_resolution(const struct motion_sensor_t *s)
@@ -70,7 +71,7 @@ static int accel_get_rms_noise(const struct motion_sensor_t *s)
 const struct accelgyro_drv test_motion_sense = {
 	.init = accel_init,
 	.read = accel_read,
-	.get_range = accel_get_range,
+	.set_range = accel_set_range,
 	.get_resolution = accel_get_resolution,
 	.set_data_rate = accel_set_data_rate,
 	.get_data_rate = accel_get_data_rate,
diff --git a/test/motion_lid.c b/test/motion_lid.c
index a9ede3a651..2c6c216430 100644
--- a/test/motion_lid.c
+++ b/test/motion_lid.c
@@ -36,7 +36,7 @@ extern enum chipset_state_mask sensor_active;
 
 /*****************************************************************************/
 /* Mock functions */
-static int accel_init(const struct motion_sensor_t *s)
+static int accel_init(struct motion_sensor_t *s)
 {
 	return EC_SUCCESS;
 }
@@ -47,18 +47,14 @@ static int accel_read(const struct motion_sensor_t *s, intv3_t v)
 	return EC_SUCCESS;
 }
 
-static int accel_set_range(const struct motion_sensor_t *s,
+static int accel_set_range(struct motion_sensor_t *s,
 			   const int range,
 			   const int rnd)
 {
+	s->current_range = range;
 	return EC_SUCCESS;
 }
 
-static int accel_get_range(const struct motion_sensor_t *s)
-{
-	return s->default_range;
-}
-
 static int accel_get_resolution(const struct motion_sensor_t *s)
 {
 	return 0;
@@ -83,7 +79,6 @@ const struct accelgyro_drv test_motion_sense = {
 	.init = accel_init,
 	.read = accel_read,
 	.set_range = accel_set_range,
-	.get_range = accel_get_range,
 	.get_resolution = accel_get_resolution,
 	.set_data_rate = accel_set_data_rate,
 	.get_data_rate = accel_get_data_rate,
diff --git a/test/online_calibration.c b/test/online_calibration.c
index f0432d5893..1b3abf51bc 100644
--- a/test/online_calibration.c
+++ b/test/online_calibration.c
@@ -44,30 +44,8 @@ static int mock_read_temp(const struct motion_sensor_t *s, int *temp)
 	return EC_ERROR_UNKNOWN;
 }
 
-struct mock_get_range_result {
-	struct motion_sensor_t *s;
-	int ret;
-	struct mock_get_range_result *next;
-};
-
-static struct mock_get_range_result *mock_get_range_results;
-
-static int mock_get_range(const struct motion_sensor_t *s)
-{
-	struct mock_get_range_result *ptr = mock_get_range_results;
-
-	while (ptr) {
-		if (ptr->s == s)
-			return ptr->ret;
-		ptr = ptr->next;
-	}
-
-	return 4;
-}
-
 static struct accelgyro_drv mock_sensor_driver = {
 	.read_temp = mock_read_temp,
-	.get_range = mock_get_range,
 };
 
 static struct accel_cal_algo base_accel_cal_algos[] = {
@@ -105,6 +83,7 @@ bool accel_cal_accumulate(
 struct motion_sensor_t motion_sensors[] = {
 	[BASE] = {
 		.type = MOTIONSENSE_TYPE_ACCEL,
+		.default_range = 4,
 		.drv = &mock_sensor_driver,
 		.online_calib_data[0] = {
 			.type_specific_data = &base_accel_cal_data,
@@ -112,6 +91,7 @@ struct motion_sensor_t motion_sensors[] = {
 	},
 	[LID] = {
 		.type = MOTIONSENSE_TYPE_MAG,
+		.default_range = 4,
 		.drv = &mock_sensor_driver,
 		.online_calib_data[0] = {
 			.type_specific_data = &lid_mag_cal_data,