driver: lsm6dsm/lsm6dsl/lis2dh: Use STM MEMs common functions

LSM6DSM now use STM MEMs common functions already
used by LIS2DH12 device. Added st_raw_read_n_noinc
function to read i2c data not using auto increment
protocol.

BUG=none
BRANCH=master
TEST=Tested on discovery BOARD with LSM6DSM sensor connected on
I2C master bus interface of target board.
Using motion sense console commands is possible to enable, set data
rate, set full scale and show data from sensor (acc and gyro).
Commons function works properly for LSM6DSM.

Change-Id: I7a987306135a85abcfa9c1d3ba596fb70598fadc
Signed-off-by: Gwendal Grignou <gwendal@chromium.org>
Reviewed-on: https://chromium-review.googlesource.com/465376
Reviewed-on: https://chromium-review.googlesource.com/982282
Reviewed-by: Furquan Shaikh <furquan@chromium.org>
Commit-Queue: Furquan Shaikh <furquan@chromium.org>
Tested-by: Furquan Shaikh <furquan@chromium.org>
Trybot-Ready: Furquan Shaikh <furquan@chromium.org>

1 files changed, 38 insertions, 31 deletions

diff --git a/driver/stm_mems_common.c b/driver/stm_mems_common.c
index afffba2d25..c2939ae714 100644
--- a/driver/stm_mems_common.c
+++ b/driver/stm_mems_common.c
@@ -27,10 +27,7 @@ int raw_write8(const int port, const int addr, const int reg, int data)
 }
 
 /**
- * Read n bytes for read
- * NOTE: Some chip use MSB for auto-increments in SUB address
- * MSB must be set for autoincrement in multi read when auto_inc
- * is set
+ * st_raw_read_n - Read n bytes for read
  */
 int st_raw_read_n(const int port, const int addr, const uint8_t reg,
 	       uint8_t *data_ptr, const int len)
@@ -46,8 +43,24 @@ int st_raw_read_n(const int port, const int addr, const uint8_t reg,
 	return rv;
 }
 
+/**
+ * st_raw_read_n_noinc - Read n bytes for read (no auto inc address)
+ */
+int st_raw_read_n_noinc(const int port, const int addr, const uint8_t reg,
+	       uint8_t *data_ptr, const int len)
+{
+	int rv = -EC_ERROR_PARAM1;
+
+	/* TODO: Implement SPI interface support */
+	i2c_lock(port, 1);
+	rv = i2c_xfer(port, addr, &reg, 1, data_ptr, len, I2C_XFER_SINGLE);
+	i2c_lock(port, 0);
+
+	return rv;
+}
+
  /**
- * write_data_with_mask - Write register with mask
+ * st_write_data_with_mask - Write register with mask
  * @s: Motion sensor pointer
  * @reg: Device register
  * @mask: The mask to search
@@ -72,11 +85,9 @@ int st_write_data_with_mask(const struct motion_sensor_t *s, int reg,
 	return raw_write8(s->port, s->addr, reg, new_data);
 }
 
- /**
- * get_resolution - Get bit resolution
+/**
+ * st_get_resolution - Get bit resolution
  * @s: Motion sensor pointer
- *
- * TODO: must support multiple resolution
  */
 int st_get_resolution(const struct motion_sensor_t *s)
 {
@@ -86,7 +97,7 @@ int st_get_resolution(const struct motion_sensor_t *s)
 }
 
 /**
- * set_offset - Set data offset
+ * st_set_offset - Set data offset
  * @s: Motion sensor pointer
  * @offset: offset vector
  * @temp: Temp
@@ -103,7 +114,7 @@ int st_set_offset(const struct motion_sensor_t *s,
 }
 
 /**
- * get_offset - Get data offset
+ * st_get_offset - Get data offset
  * @s: Motion sensor pointer
  * @offset: offset vector
  * @temp: Temp
@@ -121,7 +132,7 @@ int st_get_offset(const struct motion_sensor_t *s,
 }
 
 /**
- * get_data_rate - Get data rate (ODR)
+ * st_get_data_rate - Get data rate (ODR)
  * @s: Motion sensor pointer
  */
 int st_get_data_rate(const struct motion_sensor_t *s)
@@ -132,34 +143,30 @@ int st_get_data_rate(const struct motion_sensor_t *s)
 }
 
 /**
- * normalize - Apply LSB data sens. and rotation based on sensor resolution
+ * st_normalize - Apply LSB data sens. and rotation based on sensor resolution
  * @s: Motion sensor pointer
  * @v: output vector
  * @data: LSB raw data
  */
 void st_normalize(const struct motion_sensor_t *s, vector_3_t v, uint8_t *data)
 {
-	int i;
+	int i, range;
 	struct stprivate_data *drvdata = s->drv_data;
 
-	/* Adjust data to sensor Sensitivity and Precision:
-	 * - devices with 16 bits resolution has gain in ug/LSB
-	 * - devices with 8/10 bits resolution has gain in mg/LSB
-	 */
-	for (i = 0; i < 3; i++) {
-		switch (drvdata->resol) {
-		case 10:
-			v[i] = ((int16_t)((data[i * 2 + 1] << 8) |
-				   data[i * 2]) >> 6);
-			v[i] = v[i] * drvdata->base.range;
-			break;
-		case 16:
-			v[i] = ((int16_t)(data[i * 2 + 1] << 8) |
-				    data[i * 2]);
-			v[i] = (v[i] * drvdata->base.range) / 1000;
-			break;
-		}
+	for (i = X; i <= Z; i++) {
+		v[i] = (int16_t)((data[i * 2 + 1] << 8) |
+				data[i * 2]) >> (16 - drvdata->resol);
 	}
 
 	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++)
+		/*
+		 * TODO(gwendal): only work for accel:
+		 * offsest is in 1/1024th g/LSB, so shift left and divide by
+		 * range to normalize the value.
+		 */
+		v[i] += (drvdata->offset[i] << 5) / range;
 }