#include <linux/printk.h>
#include <linux/psci.h>
#include <linux/reboot.h>
+ #include <linux/slab.h>
#include <linux/suspend.h>
+#include <linux/regulator/machine.h>
#include <uapi/linux/psci.h>
#include <asm/smp_plat.h>
#include <asm/suspend.h>
++#ifdef CONFIG_FIQ_DEBUGGER_EL3_TO_EL1
++#include <linux/rockchip/rockchip_sip.h>
++#endif
++
/*
* While a 64-bit OS can make calls with SMC32 calling conventions, for some
* calls it is necessary to use SMC64 to pass or return 64-bit values.
psci_func_id, 0, 0);
}
+ #ifdef CONFIG_CPU_IDLE
+ static DEFINE_PER_CPU_READ_MOSTLY(u32 *, psci_power_state);
+
+ static int psci_dt_cpu_init_idle(struct device_node *cpu_node, int cpu)
+ {
+ int i, ret, count = 0;
+ u32 *psci_states;
+ struct device_node *state_node;
+
+ /*
+ * If the PSCI cpu_suspend function hook has not been initialized
+ * idle states must not be enabled, so bail out
+ */
+ if (!psci_ops.cpu_suspend)
+ return -EOPNOTSUPP;
+
+ /* Count idle states */
+ while ((state_node = of_parse_phandle(cpu_node, "cpu-idle-states",
+ count))) {
+ count++;
+ of_node_put(state_node);
+ }
+
+ if (!count)
+ return -ENODEV;
+
+ psci_states = kcalloc(count, sizeof(*psci_states), GFP_KERNEL);
+ if (!psci_states)
+ return -ENOMEM;
+
+ for (i = 0; i < count; i++) {
+ u32 state;
+
+ state_node = of_parse_phandle(cpu_node, "cpu-idle-states", i);
+
+ ret = of_property_read_u32(state_node,
+ "arm,psci-suspend-param",
+ &state);
+ if (ret) {
+ pr_warn(" * %s missing arm,psci-suspend-param property\n",
+ state_node->full_name);
+ of_node_put(state_node);
+ goto free_mem;
+ }
+
+ of_node_put(state_node);
+ pr_debug("psci-power-state %#x index %d\n", state, i);
+ if (!psci_power_state_is_valid(state)) {
+ pr_warn("Invalid PSCI power state %#x\n", state);
+ ret = -EINVAL;
+ goto free_mem;
+ }
+ psci_states[i] = state;
+ }
+ /* Idle states parsed correctly, initialize per-cpu pointer */
+ per_cpu(psci_power_state, cpu) = psci_states;
+ return 0;
+
+ free_mem:
+ kfree(psci_states);
+ return ret;
+ }
+
+ int psci_cpu_init_idle(unsigned int cpu)
+ {
+ struct device_node *cpu_node;
+ int ret;
+
+ cpu_node = of_get_cpu_node(cpu, NULL);
+ if (!cpu_node)
+ return -ENODEV;
+
+ ret = psci_dt_cpu_init_idle(cpu_node, cpu);
+
+ of_node_put(cpu_node);
+
+ return ret;
+ }
+
+ static int psci_suspend_finisher(unsigned long index)
+ {
+ u32 *state = __this_cpu_read(psci_power_state);
+
+ return psci_ops.cpu_suspend(state[index - 1],
+ virt_to_phys(cpu_resume));
+ }
+
+ int psci_cpu_suspend_enter(unsigned long index)
+ {
+ int ret;
+ u32 *state = __this_cpu_read(psci_power_state);
+ /*
+ * idle state index 0 corresponds to wfi, should never be called
+ * from the cpu_suspend operations
+ */
+ if (WARN_ON_ONCE(!index))
+ return -EINVAL;
+
+ if (!psci_power_state_loses_context(state[index - 1]))
+ ret = psci_ops.cpu_suspend(state[index - 1], 0);
+ else
+ ret = cpu_suspend(index, psci_suspend_finisher);
+
++#ifdef CONFIG_FIQ_DEBUGGER_EL3_TO_EL1
++ psci_enable_fiq();
++#endif
+ return ret;
+ }
+
+ /* ARM specific CPU idle operations */
+ #ifdef CONFIG_ARM
+ static struct cpuidle_ops psci_cpuidle_ops __initdata = {
+ .suspend = psci_cpu_suspend_enter,
+ .init = psci_dt_cpu_init_idle,
+ };
+
+ CPUIDLE_METHOD_OF_DECLARE(psci, "arm,psci", &psci_cpuidle_ops);
+ #endif
+ #endif
+
static int psci_system_suspend(unsigned long unused)
{
return invoke_psci_fn(PSCI_FN_NATIVE(1_0, SYSTEM_SUSPEND),