drvier: lsm6dsm: Populate Gyroscope scale properly

Range of 250/1000/2000 dps is an approximation.
The Gyrscope uses a slighly higher range:
range     | gain(udps/LSB) | actual value(dps)
250       | 8750           |  286.72
500       | 17500          |  573.44
1000      | 35000          |  1146.88
2000      | 70000          |  2293.76

Returns the actual value for a given range.

BUG=b:121279721
BRANCH=octopus
TEST=Check scale returns the correct value:
cd /sys/bus/iio/devices/...
for i in 250 500 1000 2000 ; do echo $i > scale ; V=$(cat scale) ; echo
-n "$i: " ; echo -n "$V: " ; echo $V | python -c 'import sys
for line in sys.stdin:
  print float(line) * 32768 * 180 / 3.14159' ; done
  250: 0.000152331: 285.996835182
  500: 0.000305197: 572.998116648
  1000: 0.000610395: 1145.99811077
  2000: 0.001221325: 2293.00066781

Check CTS Verifier Gyroscope Measurement Test pass.

Change-Id: I76c977140321d01702af16f58a3dfb7036673014
Signed-off-by: Gwendal Grignou <gwendal@chromium.org>
Reviewed-on: https://chromium-review.googlesource.com/c/1423597
Reviewed-by: Enrico Granata <egranata@chromium.org>

2 files changed, 22 insertions, 36 deletions

diff --git a/driver/accelgyro_lsm6dsm.c b/driver/accelgyro_lsm6dsm.c
index 7f9ac5b983..30650eebe7 100644
--- a/driver/accelgyro_lsm6dsm.c
+++ b/driver/accelgyro_lsm6dsm.c
@@ -375,22 +375,20 @@ static int set_range(const struct motion_sensor_t *s, int range, int rnd)
 		reg_val = LSM6DSM_ACCEL_FS_REG(newrange);
 	} else {
 		/* Adjust and check rounded value for gyro. */
-		if (rnd && (newrange < LSM6DSM_GYRO_NORMALIZE_FS(newrange)))
-			newrange *= 2;
-
-		if (newrange > LSM6DSM_GYRO_FS_MAX_VAL)
-			newrange = LSM6DSM_GYRO_FS_MAX_VAL;
+		reg_val = LSM6DSM_GYRO_FS_REG(range);
+		if (rnd && (range > LSM6DSM_GYRO_NORMALIZE_FS(reg_val)))
+			reg_val++;
 
-		reg_val = LSM6DSM_GYRO_FS_REG(newrange);
+		if (reg_val > LSM6DSM_GYRO_FS_MAX_REG_VAL)
+			reg_val = LSM6DSM_GYRO_FS_MAX_REG_VAL;
+		newrange = LSM6DSM_GYRO_NORMALIZE_FS(reg_val);
 	}
 
 	mutex_lock(s->mutex);
 	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 = (s->type == MOTIONSENSE_TYPE_ACCEL ?
-				    newrange :
-				    LSM6DSM_GYRO_FS_GAIN(newrange));
+		data->base.range = newrange;
 	mutex_unlock(s->mutex);
 
 	return EC_SUCCESS;
@@ -414,9 +412,7 @@ static int get_range(const struct motion_sensor_t *s)
 	 */
 	struct stprivate_data *data = s->drv_data;
 
-	if (s->type == MOTIONSENSE_TYPE_ACCEL)
-		return data->base.range;
-	return LSM6DSM_GYRO_GAIN_FS(data->base.range);
+	return data->base.range;
 }
 
 /**
diff --git a/driver/accelgyro_lsm6dsm.h b/driver/accelgyro_lsm6dsm.h
index 72c21fa5f3..0be055271e 100644
--- a/driver/accelgyro_lsm6dsm.h
+++ b/driver/accelgyro_lsm6dsm.h
@@ -195,34 +195,24 @@ struct fstatus {
 #define LSM6DSM_GYRO_FS_ADDR		0x11
 #define LSM6DSM_GYRO_FS_MASK		0x0c
 
-#define LSM6DSM_GYRO_FS_245_VAL		0x00
-#define LSM6DSM_GYRO_FS_500_VAL		0x01
-#define LSM6DSM_GYRO_FS_1000_VAL	0x02
-#define LSM6DSM_GYRO_FS_2000_VAL	0x03
 
-#define LSM6DSM_GYRO_FS_245_GAIN	8750
-#define LSM6DSM_GYRO_FS_500_GAIN	17500
-#define LSM6DSM_GYRO_FS_1000_GAIN	35000
-#define LSM6DSM_GYRO_FS_2000_GAIN	70000
-
-#define LSM6DSM_GYRO_FS_MAX_VAL		20000
-
-/* Gyro FS Gain value from selected Full Scale */
-#define LSM6DSM_GYRO_FS_GAIN(_fs) \
-	(LSM6DSM_GYRO_FS_245_GAIN << __fls(_fs / 245))
-
-/* Gyro FS Full Scale value from Gain */
-#define LSM6DSM_GYRO_GAIN_FS(_gain) \
-	(_gain == LSM6DSM_GYRO_FS_245_GAIN ? 245 : \
-	500 << (30 - __builtin_clz(_gain / LSM6DSM_GYRO_FS_245_GAIN)))
+/* Supported gyroscope ranges:
+ * name(dps) | register | gain(udps/LSB) | actual value(dps)
+ * 250       | 0        | 8750           |  286.72
+ * 500       | 1        | 17500          |  573.44
+ * 1000      | 2        | 35000          |  1146.88
+ * 2000      | 3        | 70000          |  2293.76
+ */
+#define LSM6DSM_GYRO_FS_MIN_VAL_MDPS ((8750 << 15) / 1000)
+#define LSM6DSM_GYRO_FS_MAX_REG_VAL 3
 
-/* Gyro Reg value from Full Scale */
+/* Gyro Reg value for Full Scale selection */
 #define LSM6DSM_GYRO_FS_REG(_fs) \
-	__fls(_fs / 245)
+	__fls(MAX(1, (_fs * 1000) / LSM6DSM_GYRO_FS_MIN_VAL_MDPS))
 
-/* Gyro normalized FS value from Full Scale: for Gyro Gains are not multiple */
-#define LSM6DSM_GYRO_NORMALIZE_FS(_fs) \
-	(_fs == 245 ? 245 : 500 << __fls(_fs / 500))
+/* Gyro normalized FS value from Full Scale register */
+#define LSM6DSM_GYRO_NORMALIZE_FS(_reg) \
+	((LSM6DSM_GYRO_FS_MIN_VAL_MDPS << (_reg)) / 1000)
 
 /* FS register address/mask for Acc/Gyro sensors */
 #define LSM6DSM_RANGE_REG(_sensor)  (LSM6DSM_ACCEL_FS_ADDR + (_sensor))