driver: lis2mdl: add standalone support

BRANCH=None
BUG=b:128619310
TEST=Created new console commands to directly trigger init, read, and
set_data_rate for the sensor. Manually verified behavior and register
values from the magnetometer using ISH console.

Change-Id: Ie162827f596056ee4cfd96be5c457e08708a9b9b
Signed-off-by: Yuval Peress <peress@chromium.org>
Reviewed-on: https://chromium-review.googlesource.com/1534339
Commit-Ready: Jett Rink <jettrink@chromium.org>
Reviewed-by: Jett Rink <jettrink@chromium.org>

1 files changed, 214 insertions, 9 deletions

diff --git a/driver/mag_lis2mdl.c b/driver/mag_lis2mdl.c
index 99415e9695..460d3cb81d 100644
--- a/driver/mag_lis2mdl.c
+++ b/driver/mag_lis2mdl.c
@@ -14,6 +14,8 @@
 #include "driver/sensorhub_lsm6dsm.h"
 #include "driver/accelgyro_lsm6dsm.h"
 #include "driver/stm_mems_common.h"
+#include "hwtimer.h"
+#include "mag_cal.h"
 #include "task.h"
 
 #ifdef CONFIG_MAG_LSM6DSM_LIS2MDL
@@ -22,6 +24,8 @@
 #endif
 #endif
 
+#define CPRINTF(format, args...) cprintf(CC_ACCEL, format, ## args)
+
 void lis2mdl_normalize(const struct motion_sensor_t *s,
 		       intv3_t v,
 		       uint8_t *raw)
@@ -88,10 +92,7 @@ static int get_range(const struct motion_sensor_t *s)
 static int set_offset(const struct motion_sensor_t *s,
 		      const int16_t *offset, int16_t temp)
 {
-
-#ifdef CONFIG_LSM6DSM_SEC_I2C
 	struct mag_cal_t *cal = LIS2MDL_CAL(s);
-#endif
 
 	cal->bias[X] = offset[X];
 	cal->bias[Y] = offset[Y];
@@ -109,9 +110,7 @@ static int set_offset(const struct motion_sensor_t *s,
 static int get_offset(const struct motion_sensor_t *s,
 		      int16_t *offset, int16_t *temp)
 {
-#ifdef CONFIG_LSM6DSM_SEC_I2C
 	struct mag_cal_t *cal = LIS2MDL_CAL(s);
-#endif
 	intv3_t offset_int;
 
 	rotate(cal->bias, *s->rot_standard_ref, offset_int);
@@ -159,7 +158,7 @@ int lis2mdl_thru_lsm6dsm_init(const struct motion_sensor_t *s)
 			LSM6DSM_MAIN_SENSOR(s),
 			CONFIG_ACCELGYRO_SEC_ADDR,
 			LIS2MDL_CFG_REG_A_ADDR,
-			LIS2MDL_ODR_100HZ | LIS2MDL_CONT_MODE);
+			LIS2MDL_ODR_50HZ | LIS2MDL_CONT_MODE);
 	if (ret != EC_SUCCESS)
 		goto err_unlock;
 
@@ -180,18 +179,224 @@ err_unlock:
 	mutex_unlock(s->mutex);
 	return ret;
 }
-#endif  /* CONFIG_MAG_LSM6DSM_LIS2MDL */
+
+#else /* END: CONFIG_MAG_LSM6DSM_LIS2MDL */
+
+/**
+ * Checks whether or not data is ready. If the check succeeds EC_SUCCESS will be
+ * returned and the ready target written with the axes that are available, see:
+ * <ul>
+ *   <li>LIS2MDL_X_DIRTY</li>
+ *   <li>LIS2MDL_Y_DIRTY</li>
+ *   <li>LIS2MDL_Z_DIRTY</li>
+ *   <li>LIS2MDL_XYZ_DIRTY</li>
+ * </ul>
+ *
+ * @param s Motion sensor pointer
+ * @param[out] ready Writeback pointer to store the result.
+ * @return EC_SUCCESS when the status register was read successfully.
+ */
+static int lis2mdl_is_data_ready(const struct motion_sensor_t *s, int *ready)
+{
+	int ret, tmp;
+
+	ret = st_raw_read8(s->port, s->addr, LIS2MDL_STATUS_REG, &tmp);
+	if (ret != EC_SUCCESS) {
+		*ready = 0;
+		return ret;
+	}
+
+	*ready = tmp & LIS2MDL_XYZ_DIRTY_MASK;
+	return EC_SUCCESS;
+
+}
+
+/**
+ * Read the most recent data from the sensor. If no new data is available,
+ * simply return the last available values.
+ *
+ * @param s Motion sensor pointer
+ * @param v A vector of 3 ints for x, y, z values.
+ * @return EC_SUCCESS when the values were read successfully or no new data was
+ * available.
+ */
+int lis2mdl_read(const struct motion_sensor_t *s, intv3_t v)
+{
+	int ret = EC_SUCCESS, ready = 0;
+	uint8_t raw[OUT_XYZ_SIZE];
+
+	ret = lis2mdl_is_data_ready(s, &ready);
+	if (ret != EC_SUCCESS)
+		return ret;
+
+	/*
+	 * If sensor data is not ready, return the previous read data.
+	 * Note: return success so that the motion sensor task can read again to
+	 * get the latest updated sensor data quickly.
+	 */
+	if (!ready) {
+		if (v != s->raw_xyz)
+			memcpy(v, s->raw_xyz, sizeof(intv3_t));
+		return ret;
+	}
+
+	mutex_lock(s->mutex);
+	ret = st_raw_read_n(s->port, s->addr, LIS2MDL_OUT_REG, raw,
+			    OUT_XYZ_SIZE);
+	mutex_unlock(s->mutex);
+	if (ret == EC_SUCCESS) {
+		lis2mdl_normalize(s, v, raw);
+		rotate(v, *s->rot_standard_ref, v);
+	}
+	return ret;
+}
+
+/**
+ * Initialize the sensor. This function will verify the who-am-I register
+ */
+int lis2mdl_init(const struct motion_sensor_t *s)
+{
+	int ret = EC_ERROR_UNKNOWN, who_am_i, count = LIS2MDL_STARTUP_MS;
+	struct mag_cal_t *cal = LIS2MDL_CAL(s);
+
+	/* Check who am I value */
+	do {
+		ret = st_raw_read8(s->port, LIS2MDL_ADDR, LIS2MDL_WHO_AM_I_REG,
+				   &who_am_i);
+		if (ret != EC_SUCCESS) {
+			/* Make sure we wait for the chip to start up. Sleep 1ms
+			 * and try again.
+			 */
+			udelay(10);
+			count--;
+		} else {
+			break;
+		}
+	} while (count > 0);
+	if (ret != EC_SUCCESS)
+		return ret;
+	if (who_am_i != LIS2MDL_WHO_AM_I)
+		return EC_ERROR_ACCESS_DENIED;
+
+	mutex_lock(s->mutex);
+
+	/* Reset the sensor */
+	ret = st_raw_write8(s->port, LIS2MDL_ADDR, LIS2MDL_CFG_REG_A_ADDR,
+			    LIS2MDL_FLAG_SW_RESET);
+	if (ret != EC_SUCCESS)
+		goto lis2mdl_init_error;
+
+	mutex_unlock(s->mutex);
+
+	if (ret != EC_SUCCESS)
+		return ret;
+
+	init_mag_cal(cal);
+	cal->radius = 0.0f;
+	LIS2MDL_ST_DATA(s)->base.odr = 0;
+	return sensor_init_done(s);
+
+lis2mdl_init_error:
+	mutex_unlock(s->mutex);
+	return ret;
+}
+
+/**
+ * Set the data rate of the sensor. Use a rate of 0 or below to turn off the
+ * magnetometer. All other values will turn on the sensor in continuous mode.
+ * The rate will be set to the nearest available value:
+ * <ul>
+ *   <li>LIS2MDL_ODR_10HZ</li>
+ *   <li>LIS2MDL_ODR_20HZ</li>
+ *   <li>LIS2MDL_ODR_50HZ</li>
+ * </ul>
+ *
+ * @param s Motion sensor pointer
+ * @param rate Rate (mHz)
+ * @param rnd Flag used to tell whether or not to round up (1) or down (0)
+ * @return EC_SUCCESS when the rate was successfully changed.
+ */
+int lis2mdl_set_data_rate(const struct motion_sensor_t *s, int rate, int rnd)
+{
+	int ret = EC_SUCCESS, normalized_rate = 0;
+	uint8_t reg_val = 0;
+	struct mag_cal_t *cal = LIS2MDL_CAL(s);
+	struct stprivate_data *data = LIS2MDL_ST_DATA(s);
+
+	if (rate > 0) {
+		if (rnd)
+			/* Round up */
+			reg_val = rate <= 10000 ? LIS2MDL_ODR_10HZ
+				: rate <= 20000 ? LIS2MDL_ODR_20HZ
+				: LIS2MDL_ODR_50HZ;
+		else
+			/* Round down */
+			reg_val = rate < 20000 ? LIS2MDL_ODR_10HZ
+				: rate < 50000 ? LIS2MDL_ODR_20HZ
+				: LIS2MDL_ODR_50HZ;
+	}
+
+	normalized_rate = rate <= 0 ? 0
+		: reg_val == LIS2MDL_ODR_10HZ ? 10000
+		: reg_val == LIS2MDL_ODR_20HZ ? 20000
+		: 50000;
+
+	/* b/130417518 - verify why skipping init_mag_cal is needed here. */
+	if (normalized_rate == data->base.odr)
+		return ret;
+
+	init_mag_cal(cal);
+
+	if (normalized_rate > 0)
+		cal->batch_size = MAX(
+				MAG_CAL_MIN_BATCH_SIZE,
+				(normalized_rate * 1000) /
+					MAG_CAL_MIN_BATCH_WINDOW_US);
+	else
+		cal->batch_size = 0;
+
+	mutex_lock(s->mutex);
+	if (rate <= 0) {
+		ret = st_raw_write8(s->port, LIS2MDL_ADDR,
+				    LIS2MDL_CFG_REG_A_ADDR,
+				    LIS2MDL_FLAG_SW_RESET);
+	} else {
+		/* Add continuous and temp compensation flags */
+		reg_val |= LIS2MDL_MODE_CONT | LIS2MDL_FLAG_TEMP_COMPENSATION;
+		ret = st_raw_write8(s->port, LIS2MDL_ADDR,
+				    LIS2MDL_CFG_REG_A_ADDR, reg_val);
+	}
+
+	mutex_unlock(s->mutex);
+
+	if (ret == EC_SUCCESS)
+		data->base.odr = normalized_rate;
+
+	return ret;
+}
+
+int get_data_rate(const struct motion_sensor_t *s)
+{
+	return LIS2MDL_ST_DATA(s)->base.odr;
+}
+
+#endif /* CONFIG_MAG_LIS2MDL */
 
 const struct accelgyro_drv lis2mdl_drv = {
 #ifdef CONFIG_MAG_LSM6DSM_LIS2MDL
 	.init = lis2mdl_thru_lsm6dsm_init,
 	.read = lis2mdl_thru_lsm6dsm_read,
 	.set_data_rate = lsm6dsm_set_data_rate,
-#endif
+	.get_data_rate = st_get_data_rate,
+#else /* CONFIG_MAG_LSM6DSM_LIS2MDL */
+	.init = lis2mdl_init,
+	.read = lis2mdl_read,
+	.set_data_rate = lis2mdl_set_data_rate,
+	.get_data_rate = get_data_rate,
+#endif /* !CONFIG_MAG_LSM6DSM_LIS2MDL */
 	.set_range = set_range,
 	.get_range = get_range,
 	.get_resolution = st_get_resolution,
-	.get_data_rate = st_get_data_rate,
 	.set_offset = set_offset,
 	.get_offset = get_offset,
 };