pm: device: Dynamically add a device to a power domain

Add API to add devices to a power domain in runtime. The number of
devices that can be added is defined in build time.

The script gen_handles.py will check the number defined in
`CONFIG_PM_DEVICE_POWER_DOMAIN_DYNAMIC` to resize the handles vector,
adding empty slots in the supported sector to be used later.

Signed-off-by: Flavio Ceolin <flavio.ceolin@intel.com>

Author: Flavio Ceolin

CMakeLists.txt

@@ -840,6 +840,12 @@ zephyr_get_include_directories_for_lang(C
   STRIP_PREFIX # Don't use a -I prefix
   )
 
+if(CONFIG_PM_DEVICE_POWER_DOMAIN_DYNAMIC)
+  set(number_of_dynamic_devices ${CONFIG_PM_DEVICE_POWER_DOMAIN_DYNAMIC_NUM})
+else()
+  set(number_of_dynamic_devices 0)
+endif()
+
 if(CONFIG_HAS_DTS)
   # dev_handles.c is generated from ${ZEPHYR_LINK_STAGE_EXECUTABLE} by
   # gen_handles.py
@@ -849,6 +855,7 @@ if(CONFIG_HAS_DTS)
     ${PYTHON_EXECUTABLE}
     ${ZEPHYR_BASE}/scripts/gen_handles.py
     --output-source dev_handles.c
+    --num-dynamic-devices ${number_of_dynamic_devices}
     --kernel $<TARGET_FILE:${ZEPHYR_LINK_STAGE_EXECUTABLE}>
     --zephyr-base ${ZEPHYR_BASE}
     --start-symbol "$<TARGET_PROPERTY:linker,devices_start_symbol>"

include/zephyr/device.h

@@ -444,6 +444,12 @@ struct device_state {
 
 struct pm_device;
 
+#ifdef CONFIG_HAS_DYNAMIC_DEVICE_HANDLES
+#define Z_DEVICE_HANDLES_CONST
+#else
+#define Z_DEVICE_HANDLES_CONST const
+#endif
+
 /**
  * @brief Runtime device structure (in ROM) per driver instance
  */
@@ -465,7 +471,8 @@ struct device {
 	 * extracted with dedicated API, such as
 	 * device_required_handles_get().
 	 */
-	const device_handle_t *const handles;
+	Z_DEVICE_HANDLES_CONST device_handle_t * const handles;
+
 #ifdef CONFIG_PM_DEVICE
 	/** Reference to the device PM resources. */
 	struct pm_device * const pm;
@@ -877,9 +884,9 @@ __deprecated static inline int device_usable_check(const struct device *dev)
  */
 BUILD_ASSERT(sizeof(device_handle_t) == 2, "fix the linker scripts");
 #define Z_DEVICE_DEFINE_HANDLES(node_id, dev_name, ...)			\
-	extern const device_handle_t					\
+	extern Z_DEVICE_HANDLES_CONST device_handle_t			\
 		Z_DEVICE_HANDLE_NAME(node_id, dev_name)[];		\
-	const device_handle_t						\
+	Z_DEVICE_HANDLES_CONST device_handle_t				\
 	__aligned(sizeof(device_handle_t))				\
 	__attribute__((__weak__,					\
 		       __section__(".__device_handles_pass1")))		\

include/zephyr/linker/common-ram.ld

@@ -45,6 +45,19 @@
 	} GROUP_DATA_LINK_IN(RAMABLE_REGION, ROMABLE_REGION)
 #endif
 
+#if defined(CONFIG_HAS_DYNAMIC_DEVICE_HANDLES)
+	SECTION_DATA_PROLOGUE(device_handles,,)
+	{
+	__device_handles_start = .;
+#ifdef LINKER_DEVICE_HANDLES_PASS1
+	KEEP(*(SORT(.__device_handles_pass1*)));
+#else
+	KEEP(*(SORT(.__device_handles_pass2*)));
+#endif /* LINKER_DEVICE_HANDLES_PASS1 */
+	__device_handles_end = .;
+	} GROUP_DATA_LINK_IN(RAMABLE_REGION, ROMABLE_REGION)
+#endif 	/* CONFIG_HAS_DYNAMIC_DEVICE_HANDLES */
+
 	SECTION_DATA_PROLOGUE(initshell,,)
 	{
 		/* link in shell initialization objects for all modules that

include/zephyr/linker/common-rom.ld

@@ -220,6 +220,7 @@
 		KEEP(*(".dbg_thread_info"));
 	} GROUP_ROM_LINK_IN(RAMABLE_REGION, ROMABLE_REGION)
 
+#if !defined(CONFIG_HAS_DYNAMIC_DEVICE_HANDLES)
 	SECTION_DATA_PROLOGUE(device_handles,,)
 	{
 		__device_handles_start = .;
@@ -230,3 +231,4 @@
 #endif /* LINKER_DEVICE_HANDLES_PASS1 */
 		__device_handles_end = .;
 	} GROUP_ROM_LINK_IN(RAMABLE_REGION, ROMABLE_REGION)
+#endif /* !CONFIG_HAS_DYNAMIC_DEVICE_HANDLES */

include/zephyr/pm/device.h

@@ -521,6 +521,36 @@ bool pm_device_state_is_locked(const struct device *dev);
  */
 bool pm_device_on_power_domain(const struct device *dev);
 
+/**
+ * @brief Add a device to a power domain.
+ *
+ * This function adds a device to a given power domain.
+ *
+ * @param dev Device to be added to the power domain.
+ * @param domain Power domain.
+ *
+ * @retval 0 If successful.
+ * @retval -EALREADY If device is already part of the power domain.
+ * @retval -ENOSYS If the application was built without power domain support.
+ * @retval -ENOSPC If there is no space available in the power domain to add the device.
+ */
+int pm_device_power_domain_add(const struct device *dev,
+			       const struct device *domain);
+
+/**
+ * @brief Remove a device from a power domain.
+ *
+ * This function removes a device from a given power domain.
+ *
+ * @param dev Device to be removed from the power domain.
+ * @param domain Power domain.
+ *
+ * @retval 0 If successful.
+ * @retval -ENOSYS If the application was built without power domain support.
+ * @retval -ENOENT If device is not in the given domain.
+ */
+int pm_device_power_domain_remove(const struct device *dev,
+				  const struct device *domain);
 #else
 static inline void pm_device_init_suspended(const struct device *dev)
 {
@@ -579,6 +609,19 @@ static inline bool pm_device_on_power_domain(const struct device *dev)
 	ARG_UNUSED(dev);
 	return false;
 }
+
+static inline int pm_device_power_domain_add(const struct device *dev,
+					     const struct device *domain)
+{
+	return -ENOSYS;
+}
+
+static inline int pm_device_power_domain_remove(const struct device *dev,
+						const struct device *domain)
+{
+	return -ENOSYS;
+}
+
 #endif /* CONFIG_PM_DEVICE */
 
 /** @} */

kernel/Kconfig

@@ -928,4 +928,14 @@ config THREAD_LOCAL_STORAGE
 
 endmenu
 
+menu "Device Options"
+
+config HAS_DYNAMIC_DEVICE_HANDLES
+	bool
+	help
+	  Hidden option that makes possible to manipulate device handles at
+	  runtime.
+
+endmenu
+
 rsource "Kconfig.vm"

scripts/gen_handles.py

@@ -62,6 +62,8 @@ def parse_args():
 
     parser.add_argument("-k", "--kernel", required=True,
                         help="Input zephyr ELF binary")
+    parser.add_argument("-d", "--num-dynamic-devices", required=False, default=0,
+                        type=int, help="Input number of dynamic devices allowed")
     parser.add_argument("-o", "--output-source", required=True,
             help="Output source file")
 
@@ -112,6 +114,7 @@ def symbol_handle_data(elf, sym):
 # These match the corresponding constants in <device.h>
 DEVICE_HANDLE_SEP = -32768
 DEVICE_HANDLE_ENDS = 32767
+DEVICE_HANDLE_NULL = 0
 def handle_name(hdl):
     if hdl == DEVICE_HANDLE_SEP:
         return "DEVICE_HANDLE_SEP"
@@ -336,6 +339,7 @@ def main():
                     else:
                         sup_paths.append('(%s)' % dn.path)
                 hdls.extend(dn.__device.dev_handle for dn in sn.__supports)
+            hdls.extend(DEVICE_HANDLE_NULL for dn in range(args.num_dynamic_devices))
 
             # Terminate the array with the end symbol
             hdls.append(DEVICE_HANDLE_ENDS)

subsys/pm/Kconfig

@@ -82,6 +82,20 @@ config PM_DEVICE_POWER_DOMAIN
 	  devices that depend on a domain will be notified when this
 	  domain is suspended or resumed.
 
+config PM_DEVICE_POWER_DOMAIN_DYNAMIC
+	bool "Dynamically bind devices to a Power Pomain"
+	depends on PM_DEVICE_POWER_DOMAIN
+	select HAS_DYNAMIC_DEVICE_HANDLES
+	help
+	  Enable support for dynamically bind devices to a Power Domain.
+
+config PM_DEVICE_POWER_DOMAIN_DYNAMIC_NUM
+	int "Number of devices that can dynamically be bind to a Power Domain"
+	depends on PM_DEVICE_POWER_DOMAIN_DYNAMIC
+	default 1
+	help
+	  The number of devices that can dynamically be bind to a Power Domain.
+
 config PM_DEVICE_RUNTIME
 	bool "Runtime Device Power Management"
 	help

subsys/pm/device.c

@@ -101,6 +101,82 @@ int pm_device_action_run(const struct device *dev,
 	return 0;
 }
 
+static int power_domain_add_or_remove(const struct device *dev,
+				      const struct device *domain,
+				      bool add)
+{
+#if defined(CONFIG_HAS_DYNAMIC_DEVICE_HANDLES)
+	device_handle_t *rv = domain->handles;
+	device_handle_t dev_handle = -1;
+	extern const struct device __device_start[];
+	extern const struct device __device_end[];
+	size_t i, region = 0;
+	size_t numdev = __device_end - __device_start;
+
+	/*
+	 * Supported devices are stored as device handle and not
+	 * device pointers. So, it is necessary to find what is
+	 * the handle associated to the given device.
+	 */
+	for (i = 0; i < numdev; i++) {
+		if (&__device_start[i] == dev) {
+			dev_handle = i + 1;
+			break;
+		}
+	}
+
+	/*
+	 * The last part is to find an available slot in the
+	 * supported section of handles array and replace it
+	 * with the device handle.
+	 */
+	while (region != 2) {
+		if (*rv == DEVICE_HANDLE_SEP) {
+			region++;
+		}
+		rv++;
+	}
+
+	i = 0;
+	while (rv[i] != DEVICE_HANDLE_ENDS) {
+		if (add == false) {
+			if (rv[i] == dev_handle) {
+				dev->pm->domain = NULL;
+				rv[i] = DEVICE_HANDLE_NULL;
+				return 0;
+			}
+		} else {
+			if (rv[i] == DEVICE_HANDLE_NULL) {
+				dev->pm->domain = domain;
+				rv[i] = dev_handle;
+				return 0;
+			}
+		}
+		++i;
+	}
+
+	return add ? -ENOSPC : -ENOENT;
+#else
+	ARG_UNUSED(dev);
+	ARG_UNUSED(domain);
+	ARG_UNUSED(add);
+
+	return -ENOSYS;
+#endif
+}
+
+int pm_device_power_domain_remove(const struct device *dev,
+				  const struct device *domain)
+{
+	return power_domain_add_or_remove(dev, domain, false);
+}
+
+int pm_device_power_domain_add(const struct device *dev,
+			       const struct device *domain)
+{
+	return power_domain_add_or_remove(dev, domain, true);
+}
+
 void pm_device_children_action_run(const struct device *dev,
 				   enum pm_device_action action,
 				   pm_device_action_failed_cb_t failure_cb)
@@ -121,6 +197,10 @@ void pm_device_children_action_run(const struct device *dev,
 		device_handle_t dh = handles[i];
 		const struct device *cdev = device_from_handle(dh);
 
+		if (cdev == NULL) {
+			continue;
+		}
+
 		rc = pm_device_action_run(cdev, action);
 		if ((failure_cb != NULL) && (rc < 0)) {
 			/* Stop the iteration if the callback requests it */