From e75868db322619abeafa0971da7368b6143aa8dd Mon Sep 17 00:00:00 2001
From: Vincent Dupont <vincent@otakeys.com>
Date: Wed, 9 Aug 2017 09:53:05 +0200
Subject: [PATCH] drivers/lsm6dsl: add power up/down functions

---
 drivers/include/lsm6dsl.h |  40 ++++++++++++++
 drivers/lsm6dsl/lsm6dsl.c | 106 ++++++++++++++++++++++++++++++++++++++
 2 files changed, 146 insertions(+)

diff --git a/drivers/include/lsm6dsl.h b/drivers/include/lsm6dsl.h
index 6b98261d28..6cd1f01a7b 100644
--- a/drivers/include/lsm6dsl.h
+++ b/drivers/include/lsm6dsl.h
@@ -173,6 +173,46 @@ int lsm6dsl_read_gyro(const lsm6dsl_t *dev, lsm6dsl_3d_data_t *data);
  */
 int lsm6dsl_read_temp(const lsm6dsl_t *dev, int16_t *data);
 
+/**
+ * @brief Power down accelerometer
+ *
+ * @param[in] dev    device to power down
+ *
+ * @return LSM6DSL_OK on success
+ * @return < 0 on error
+ */
+int lsm6dsl_acc_power_down(const lsm6dsl_t *dev);
+
+/**
+ * @brief Power down gyroscope
+ *
+ * @param[in] dev    device to power down
+ *
+ * @return LSM6DSL_OK on success
+ * @return < 0 on error
+ */
+int lsm6dsl_gyro_power_down(const lsm6dsl_t *dev);
+
+/**
+ * @brief Power up accelerometer
+ *
+ * @param[in] dev    device to power up
+ *
+ * @return LSM6DSL_OK on success
+ * @return < 0 on error
+ */
+int lsm6dsl_acc_power_up(const lsm6dsl_t *dev);
+
+/**
+ * @brief Power up gyroscope
+ *
+ * @param[in] dev    device to power up
+ *
+ * @return LSM6DSL_OK on success
+ * @return < 0 on error
+ */
+int lsm6dsl_gyro_power_up(const lsm6dsl_t *dev);
+
 #ifdef __cplusplus
 }
 #endif
diff --git a/drivers/lsm6dsl/lsm6dsl.c b/drivers/lsm6dsl/lsm6dsl.c
index cecf13c23c..863322d3c4 100644
--- a/drivers/lsm6dsl/lsm6dsl.c
+++ b/drivers/lsm6dsl/lsm6dsl.c
@@ -195,3 +195,109 @@ int lsm6dsl_read_temp(const lsm6dsl_t *dev, int16_t *data)
 
     return LSM6DSL_OK;
 }
+
+int lsm6dsl_acc_power_down(const lsm6dsl_t *dev)
+{
+    int res;
+    uint8_t tmp;
+
+    i2c_acquire(BUS);
+    res = i2c_read_reg(BUS, ADDR, LSM6DSL_REG_CTRL1_XL, &tmp);
+    if (res != 1) {
+        i2c_release(BUS);
+        DEBUG("[ERROR] lsm6dsl_acc_power_down\n");
+        return -LSM6DSL_ERROR_BUS;
+    }
+
+    tmp &= ~(LSM6DSL_CTRL_ODR_MASK);
+    res = i2c_write_reg(BUS, ADDR, LSM6DSL_REG_CTRL1_XL, tmp);
+
+    i2c_release(BUS);
+
+    if (res != 1) {
+        DEBUG("[ERROR] lsm6dsl_acc_power_down\n");
+        return -LSM6DSL_ERROR_BUS;
+    }
+
+    return LSM6DSL_OK;
+}
+
+int lsm6dsl_gyro_power_down(const lsm6dsl_t *dev)
+{
+    int res;
+    uint8_t tmp;
+
+    i2c_acquire(BUS);
+    res = i2c_read_reg(BUS, ADDR, LSM6DSL_REG_CTRL2_G, &tmp);
+    if (res != 1) {
+        i2c_release(BUS);
+        DEBUG("[ERROR] lsm6dsl_gyro_power_down\n");
+        return -LSM6DSL_ERROR_BUS;
+    }
+
+    tmp &= ~(LSM6DSL_CTRL_ODR_MASK);
+    res = i2c_write_reg(BUS, ADDR, LSM6DSL_REG_CTRL2_G, tmp);
+
+    i2c_release(BUS);
+
+    if (res != 1) {
+        DEBUG("[ERROR] lsm6dsl_gyro_power_down\n");
+        return -LSM6DSL_ERROR_BUS;
+    }
+
+    return LSM6DSL_OK;
+}
+
+int lsm6dsl_acc_power_up(const lsm6dsl_t *dev)
+{
+    int res;
+    uint8_t tmp;
+
+    i2c_acquire(BUS);
+    res = i2c_read_reg(BUS, ADDR, LSM6DSL_REG_CTRL1_XL, &tmp);
+    if (res != 1) {
+        i2c_release(BUS);
+        DEBUG("[ERROR] lsm6dsl_acc_power_up\n");
+        return -LSM6DSL_ERROR_BUS;
+    }
+
+    tmp &= ~(LSM6DSL_CTRL_ODR_MASK);
+    tmp |= dev->params.acc_odr << LSM6DSL_CTRL_ODR_SHIFT;
+    res = i2c_write_reg(BUS, ADDR, LSM6DSL_REG_CTRL1_XL, tmp);
+
+    i2c_release(BUS);
+
+    if (res != 1) {
+        DEBUG("[ERROR] lsm6dsl_acc_power_up\n");
+        return -LSM6DSL_ERROR_BUS;
+    }
+
+    return LSM6DSL_OK;
+}
+
+int lsm6dsl_gyro_power_up(const lsm6dsl_t *dev)
+{
+    int res;
+    uint8_t tmp;
+
+    i2c_acquire(BUS);
+    res = i2c_read_reg(BUS, ADDR, LSM6DSL_REG_CTRL2_G, &tmp);
+    if (res != 1) {
+        i2c_release(BUS);
+        DEBUG("[ERROR] lsm6dsl_gyro_power_up\n");
+        return -LSM6DSL_ERROR_BUS;
+    }
+
+    tmp &= ~(LSM6DSL_CTRL_ODR_MASK);
+    tmp |= dev->params.gyro_odr << LSM6DSL_CTRL_ODR_SHIFT;
+    res = i2c_write_reg(BUS, ADDR, LSM6DSL_REG_CTRL2_G, tmp);
+
+    i2c_release(BUS);
+
+    if (res != 1) {
+        DEBUG("[ERROR] lsm6dsl_gyro_power_up\n");
+        return -LSM6DSL_ERROR_BUS;
+    }
+
+    return LSM6DSL_OK;
+}