1 files changed, 85 insertions, 0 deletions
diff --git a/lib/utils/fdt/fdt_fixup.c b/lib/utils/fdt/fdt_fixup.c
index 42692cc4..e57a4e1d 100644
--- a/lib/utils/fdt/fdt_fixup.c
+++ b/lib/utils/fdt/fdt_fixup.c
@@ -18,6 +18,91 @@
 #include <sbi_utils/fdt/fdt_pmu.h>
 #include <sbi_utils/fdt/fdt_helper.h>
 
+int fdt_add_cpu_idle_states(void *fdt, const struct sbi_cpu_idle_state *state)
+{
+	int cpu_node, cpus_node, err, idle_states_node;
+	uint32_t count, phandle;
+
+	err = fdt_open_into(fdt, fdt, fdt_totalsize(fdt) + 1024);
+	if (err < 0)
+		return err;
+
+	err = fdt_find_max_phandle(fdt, &phandle);
+	phandle++;
+	if (err < 0)
+		return err;
+
+	cpus_node = fdt_path_offset(fdt, "/cpus");
+	if (cpus_node < 0)
+		return cpus_node;
+
+	/* Do nothing if the idle-states node already exists. */
+	idle_states_node = fdt_subnode_offset(fdt, cpus_node, "idle-states");
+	if (idle_states_node >= 0)
+		return 0;
+
+	/* Create the idle-states node and its child nodes. */
+	idle_states_node = fdt_add_subnode(fdt, cpus_node, "idle-states");
+	if (idle_states_node < 0)
+		return idle_states_node;
+
+	for (count = 0; state->name; count++, phandle++, state++) {
+		int idle_state_node;
+
+		idle_state_node = fdt_add_subnode(fdt, idle_states_node,
+						  state->name);
+		if (idle_state_node < 0)
+			return idle_state_node;
+
+		fdt_setprop_string(fdt, idle_state_node, "compatible",
+				   "riscv,idle-state");
+		fdt_setprop_u32(fdt, idle_state_node,
+				"riscv,sbi-suspend-param",
+				state->suspend_param);
+		if (state->local_timer_stop)
+			fdt_setprop_empty(fdt, idle_state_node,
+					  "local-timer-stop");
+		fdt_setprop_u32(fdt, idle_state_node, "entry-latency-us",
+				state->entry_latency_us);
+		fdt_setprop_u32(fdt, idle_state_node, "exit-latency-us",
+				state->exit_latency_us);
+		fdt_setprop_u32(fdt, idle_state_node, "min-residency-us",
+				state->min_residency_us);
+		if (state->wakeup_latency_us)
+			fdt_setprop_u32(fdt, idle_state_node,
+					"wakeup-latency-us",
+					state->wakeup_latency_us);
+		fdt_setprop_u32(fdt, idle_state_node, "phandle", phandle);
+	}
+
+	if (count == 0)
+		return 0;
+
+	/* Link each cpu node to the idle state nodes. */
+	fdt_for_each_subnode(cpu_node, fdt, cpus_node) {
+		const char *device_type;
+		fdt32_t *value;
+
+		/* Only process child nodes with device_type = "cpu". */
+		device_type = fdt_getprop(fdt, cpu_node, "device_type", NULL);
+		if (!device_type || strcmp(device_type, "cpu"))
+			continue;
+
+		/* Allocate space for the list of phandles. */
+		err = fdt_setprop_placeholder(fdt, cpu_node, "cpu-idle-states",
+					      count * sizeof(phandle),
+					      (void **)&value);
+		if (err < 0)
+			return err;
+
+		/* Fill in the phandles of the idle state nodes. */
+		for (uint32_t i = 0; i < count; ++i)
+			value[i] = cpu_to_fdt32(phandle - count + i);
+	}
+
+	return 0;
+}
+
 void fdt_cpu_fixup(void *fdt)
 {
 	struct sbi_domain *dom = sbi_domain_thishart_ptr();