From 8a77ad406f3eb20ccfc97050e4d07df6a6608b4c Mon Sep 17 00:00:00 2001
From: Emil Lindqvist <emil@lindq.gr>
Date: Thu, 2 Nov 2023 08:25:49 +0100
Subject: [PATCH] lsm6dsl: add pm suspend and resume to lsm6dsl

This commit implements the suspend and resume
PM API for LSM6DSL accelerometer.

Signed-off-by: Emil Lindqvist <emil@lindq.gr>
---
 drivers/sensor/lsm6dsl/lsm6dsl.c | 62 ++++++++++++++++++++++++++++++--
 drivers/sensor/lsm6dsl/lsm6dsl.h |  1 +
 2 files changed, 61 insertions(+), 2 deletions(-)

diff --git a/drivers/sensor/lsm6dsl/lsm6dsl.c b/drivers/sensor/lsm6dsl/lsm6dsl.c
index b00df0be526..0a0f3525804 100644
--- a/drivers/sensor/lsm6dsl/lsm6dsl.c
+++ b/drivers/sensor/lsm6dsl/lsm6dsl.c
@@ -18,6 +18,7 @@
 #include <zephyr/sys/byteorder.h>
 #include <zephyr/sys/__assert.h>
 #include <zephyr/logging/log.h>
+#include <zephyr/pm/device.h>
 
 #include "lsm6dsl.h"
 
@@ -26,7 +27,8 @@ LOG_MODULE_REGISTER(LSM6DSL, CONFIG_SENSOR_LOG_LEVEL);
 static const uint16_t lsm6dsl_odr_map[] = {0, 12, 26, 52, 104, 208, 416, 833,
 					1666, 3332, 6664, 1};
 
-#if defined(LSM6DSL_ACCEL_ODR_RUNTIME) || defined(LSM6DSL_GYRO_ODR_RUNTIME)
+#if defined(LSM6DSL_ACCEL_ODR_RUNTIME) || defined(LSM6DSL_GYRO_ODR_RUNTIME) ||\
+	defined(CONFIG_PM_DEVICE)
 static int lsm6dsl_freq_to_odr_val(uint16_t freq)
 {
 	size_t i;
@@ -169,6 +171,8 @@ static int lsm6dsl_gyro_set_odr_raw(const struct device *dev, uint8_t odr)
 		return -EIO;
 	}
 
+	data->gyro_freq = lsm6dsl_odr_to_freq_val(odr);
+
 	return 0;
 }
 
@@ -826,6 +830,59 @@ static int lsm6dsl_init(const struct device *dev)
 	return 0;
 }
 
+#ifdef CONFIG_PM_DEVICE
+static int lsm6dsl_pm_action(const struct device *dev,
+			     enum pm_device_action action)
+{
+	struct lsm6dsl_data *data = dev->data;
+	int ret = -EIO;
+	uint8_t accel_odr = 0;
+	uint8_t gyro_odr = 0;
+
+	switch (action) {
+	case PM_DEVICE_ACTION_RESUME:
+		/* Restore saved ODR values */
+		accel_odr = lsm6dsl_freq_to_odr_val(data->accel_freq);
+		ret = lsm6dsl_accel_set_odr_raw(dev, accel_odr);
+		if (ret < 0) {
+			LOG_ERR("Failed to resume accelerometer");
+			break;
+		}
+		gyro_odr = lsm6dsl_freq_to_odr_val(data->gyro_freq);
+		ret = lsm6dsl_gyro_set_odr_raw(dev, gyro_odr);
+		if (ret < 0) {
+			LOG_ERR("Failed to resume gyro");
+			break;
+		}
+		break;
+	case PM_DEVICE_ACTION_SUSPEND:
+		/*
+		 * Set accelerometer ODR to power-down. Don't use the direct
+		 * function to not overwrite the saved value
+		 */
+		ret = data->hw_tf->update_reg(dev, LSM6DSL_REG_CTRL1_XL,
+					      LSM6DSL_MASK_CTRL1_XL_ODR_XL, 0);
+		if (ret < 0) {
+			LOG_ERR("Failed to suspend accelerometer");
+			break;
+		}
+		/* Set gyro ODR to power-down */
+		ret = data->hw_tf->update_reg(dev, LSM6DSL_REG_CTRL2_G,
+					      LSM6DSL_MASK_CTRL2_G_ODR_G, 0);
+		if (ret < 0) {
+			LOG_ERR("Failed to suspend gyro");
+			break;
+		}
+
+		break;
+	default:
+		return -ENOTSUP;
+	}
+
+	return ret;
+}
+#endif
+
 
 #if DT_NUM_INST_STATUS_OKAY(DT_DRV_COMPAT) == 0
 #warning "LSM6DSL driver enabled without any devices"
@@ -837,9 +894,10 @@ static int lsm6dsl_init(const struct device *dev)
  */
 
 #define LSM6DSL_DEVICE_INIT(inst)					\
+	PM_DEVICE_DT_INST_DEFINE(inst, lsm6dsl_pm_action);		\
 	SENSOR_DEVICE_DT_INST_DEFINE(inst,				\
 			    lsm6dsl_init,				\
-			    NULL,					\
+			    PM_DEVICE_DT_INST_GET(inst),		\
 			    &lsm6dsl_data_##inst,			\
 			    &lsm6dsl_config_##inst,			\
 			    POST_KERNEL,				\
diff --git a/drivers/sensor/lsm6dsl/lsm6dsl.h b/drivers/sensor/lsm6dsl/lsm6dsl.h
index f424cbf2326..da176f4f692 100644
--- a/drivers/sensor/lsm6dsl/lsm6dsl.h
+++ b/drivers/sensor/lsm6dsl/lsm6dsl.h
@@ -667,6 +667,7 @@ struct lsm6dsl_data {
 #endif
 	const struct lsm6dsl_transfer_function *hw_tf;
 	uint16_t accel_freq;
+	uint16_t gyro_freq;
 
 #ifdef CONFIG_LSM6DSL_TRIGGER
 	const struct device *dev;