--- zzzz-none-000/linux-3.10.107/arch/arm/common/mcpm_entry.c 2017-06-27 09:49:32.000000000 +0000 +++ scorpion-7490-727/linux-3.10.107/arch/arm/common/mcpm_entry.c 2021-02-04 17:41:59.000000000 +0000 @@ -12,106 +12,19 @@ #include #include #include +#include #include #include #include #include +#include -extern unsigned long mcpm_entry_vectors[MAX_NR_CLUSTERS][MAX_CPUS_PER_CLUSTER]; - -void mcpm_set_entry_vector(unsigned cpu, unsigned cluster, void *ptr) -{ - unsigned long val = ptr ? virt_to_phys(ptr) : 0; - mcpm_entry_vectors[cluster][cpu] = val; - sync_cache_w(&mcpm_entry_vectors[cluster][cpu]); -} - -static const struct mcpm_platform_ops *platform_ops; - -int __init mcpm_platform_register(const struct mcpm_platform_ops *ops) -{ - if (platform_ops) - return -EBUSY; - platform_ops = ops; - return 0; -} - -int mcpm_cpu_power_up(unsigned int cpu, unsigned int cluster) -{ - if (!platform_ops) - return -EUNATCH; /* try not to shadow power_up errors */ - might_sleep(); - return platform_ops->power_up(cpu, cluster); -} - -typedef void (*phys_reset_t)(unsigned long); - -void mcpm_cpu_power_down(void) -{ - phys_reset_t phys_reset; - - BUG_ON(!platform_ops); - BUG_ON(!irqs_disabled()); - - /* - * Do this before calling into the power_down method, - * as it might not always be safe to do afterwards. - */ - setup_mm_for_reboot(); - - platform_ops->power_down(); - - /* - * It is possible for a power_up request to happen concurrently - * with a power_down request for the same CPU. In this case the - * power_down method might not be able to actually enter a - * powered down state with the WFI instruction if the power_up - * method has removed the required reset condition. The - * power_down method is then allowed to return. We must perform - * a re-entry in the kernel as if the power_up method just had - * deasserted reset on the CPU. - * - * To simplify race issues, the platform specific implementation - * must accommodate for the possibility of unordered calls to - * power_down and power_up with a usage count. Therefore, if a - * call to power_up is issued for a CPU that is not down, then - * the next call to power_down must not attempt a full shutdown - * but only do the minimum (normally disabling L1 cache and CPU - * coherency) and return just as if a concurrent power_up request - * had happened as described above. - */ - - phys_reset = (phys_reset_t)(unsigned long)virt_to_phys(cpu_reset); - phys_reset(virt_to_phys(mcpm_entry_point)); - - /* should never get here */ - BUG(); -} - -void mcpm_cpu_suspend(u64 expected_residency) -{ - phys_reset_t phys_reset; - - BUG_ON(!platform_ops); - BUG_ON(!irqs_disabled()); - - /* Very similar to mcpm_cpu_power_down() */ - setup_mm_for_reboot(); - platform_ops->suspend(expected_residency); - phys_reset = (phys_reset_t)(unsigned long)virt_to_phys(cpu_reset); - phys_reset(virt_to_phys(mcpm_entry_point)); - BUG(); -} - -int mcpm_cpu_powered_up(void) -{ - if (!platform_ops) - return -EUNATCH; - if (platform_ops->powered_up) - platform_ops->powered_up(); - return 0; -} +/* + * The public API for this code is documented in arch/arm/include/asm/mcpm.h. + * For a comprehensive description of the main algorithm used here, please + * see Documentation/arm/cluster-pm-race-avoidance.txt. + */ struct sync_struct mcpm_sync; @@ -120,7 +33,7 @@ * This must be called at the point of committing to teardown of a CPU. * The CPU cache (SCTRL.C bit) is expected to still be active. */ -void __mcpm_cpu_going_down(unsigned int cpu, unsigned int cluster) +static void __mcpm_cpu_going_down(unsigned int cpu, unsigned int cluster) { mcpm_sync.clusters[cluster].cpus[cpu].cpu = CPU_GOING_DOWN; sync_cache_w(&mcpm_sync.clusters[cluster].cpus[cpu].cpu); @@ -133,12 +46,12 @@ * The CPU cache (SCTRL.C bit) is expected to be off. * However L2 cache might or might not be active. */ -void __mcpm_cpu_down(unsigned int cpu, unsigned int cluster) +static void __mcpm_cpu_down(unsigned int cpu, unsigned int cluster) { dmb(); mcpm_sync.clusters[cluster].cpus[cpu].cpu = CPU_DOWN; sync_cache_w(&mcpm_sync.clusters[cluster].cpus[cpu].cpu); - dsb_sev(); + sev(); } /* @@ -149,12 +62,12 @@ * CLUSTER_DOWN: the cluster has been torn-down, ready for power-off * (CPU cache disabled, L2 cache either enabled or disabled). */ -void __mcpm_outbound_leave_critical(unsigned int cluster, int state) +static void __mcpm_outbound_leave_critical(unsigned int cluster, int state) { dmb(); mcpm_sync.clusters[cluster].cluster = state; sync_cache_w(&mcpm_sync.clusters[cluster].cluster); - dsb_sev(); + sev(); } /* @@ -168,7 +81,7 @@ * true: the critical section was entered: it is now safe to tear down the * cluster. */ -bool __mcpm_outbound_enter_critical(unsigned int cpu, unsigned int cluster) +static bool __mcpm_outbound_enter_critical(unsigned int cpu, unsigned int cluster) { unsigned int i; struct mcpm_sync_struct *c = &mcpm_sync.clusters[cluster]; @@ -221,12 +134,291 @@ return false; } -int __mcpm_cluster_state(unsigned int cluster) +static int __mcpm_cluster_state(unsigned int cluster) { sync_cache_r(&mcpm_sync.clusters[cluster].cluster); return mcpm_sync.clusters[cluster].cluster; } +extern unsigned long mcpm_entry_vectors[MAX_NR_CLUSTERS][MAX_CPUS_PER_CLUSTER]; + +void mcpm_set_entry_vector(unsigned cpu, unsigned cluster, void *ptr) +{ + unsigned long val = ptr ? virt_to_phys(ptr) : 0; + mcpm_entry_vectors[cluster][cpu] = val; + sync_cache_w(&mcpm_entry_vectors[cluster][cpu]); +} + +extern unsigned long mcpm_entry_early_pokes[MAX_NR_CLUSTERS][MAX_CPUS_PER_CLUSTER][2]; + +void mcpm_set_early_poke(unsigned cpu, unsigned cluster, + unsigned long poke_phys_addr, unsigned long poke_val) +{ + unsigned long *poke = &mcpm_entry_early_pokes[cluster][cpu][0]; + poke[0] = poke_phys_addr; + poke[1] = poke_val; + __sync_cache_range_w(poke, 2 * sizeof(*poke)); +} + +static const struct mcpm_platform_ops *platform_ops; + +int __init mcpm_platform_register(const struct mcpm_platform_ops *ops) +{ + if (platform_ops) + return -EBUSY; + platform_ops = ops; + return 0; +} + +bool mcpm_is_available(void) +{ + return (platform_ops) ? true : false; +} + +/* + * We can't use regular spinlocks. In the switcher case, it is possible + * for an outbound CPU to call power_down() after its inbound counterpart + * is already live using the same logical CPU number which trips lockdep + * debugging. + */ +static arch_spinlock_t mcpm_lock = __ARCH_SPIN_LOCK_UNLOCKED; + +static int mcpm_cpu_use_count[MAX_NR_CLUSTERS][MAX_CPUS_PER_CLUSTER]; + +static inline bool mcpm_cluster_unused(unsigned int cluster) +{ + int i, cnt; + for (i = 0, cnt = 0; i < MAX_CPUS_PER_CLUSTER; i++) + cnt |= mcpm_cpu_use_count[cluster][i]; + return !cnt; +} + +int mcpm_cpu_power_up(unsigned int cpu, unsigned int cluster) +{ + bool cpu_is_down, cluster_is_down; + int ret = 0; + + pr_debug("%s: cpu %u cluster %u\n", __func__, cpu, cluster); + if (!platform_ops) + return -EUNATCH; /* try not to shadow power_up errors */ + might_sleep(); + + /* + * Since this is called with IRQs enabled, and no arch_spin_lock_irq + * variant exists, we need to disable IRQs manually here. + */ + local_irq_disable(); + arch_spin_lock(&mcpm_lock); + + cpu_is_down = !mcpm_cpu_use_count[cluster][cpu]; + cluster_is_down = mcpm_cluster_unused(cluster); + + mcpm_cpu_use_count[cluster][cpu]++; + /* + * The only possible values are: + * 0 = CPU down + * 1 = CPU (still) up + * 2 = CPU requested to be up before it had a chance + * to actually make itself down. + * Any other value is a bug. + */ + BUG_ON(mcpm_cpu_use_count[cluster][cpu] != 1 && + mcpm_cpu_use_count[cluster][cpu] != 2); + + if (cluster_is_down) + ret = platform_ops->cluster_powerup(cluster); + if (cpu_is_down && !ret) + ret = platform_ops->cpu_powerup(cpu, cluster); + + arch_spin_unlock(&mcpm_lock); + local_irq_enable(); + return ret; +} + +typedef void (*phys_reset_t)(unsigned long); + +void mcpm_cpu_power_down(void) +{ + unsigned int mpidr, cpu, cluster; + bool cpu_going_down, last_man; + phys_reset_t phys_reset; + + mpidr = read_cpuid_mpidr(); + cpu = MPIDR_AFFINITY_LEVEL(mpidr, 0); + cluster = MPIDR_AFFINITY_LEVEL(mpidr, 1); + pr_debug("%s: cpu %u cluster %u\n", __func__, cpu, cluster); + if (WARN_ON_ONCE(!platform_ops)) + return; + BUG_ON(!irqs_disabled()); + + setup_mm_for_reboot(); + + __mcpm_cpu_going_down(cpu, cluster); + arch_spin_lock(&mcpm_lock); + BUG_ON(__mcpm_cluster_state(cluster) != CLUSTER_UP); + + mcpm_cpu_use_count[cluster][cpu]--; + BUG_ON(mcpm_cpu_use_count[cluster][cpu] != 0 && + mcpm_cpu_use_count[cluster][cpu] != 1); + cpu_going_down = !mcpm_cpu_use_count[cluster][cpu]; + last_man = mcpm_cluster_unused(cluster); + + if (last_man && __mcpm_outbound_enter_critical(cpu, cluster)) { + platform_ops->cpu_powerdown_prepare(cpu, cluster); + platform_ops->cluster_powerdown_prepare(cluster); + arch_spin_unlock(&mcpm_lock); + platform_ops->cluster_cache_disable(); + __mcpm_outbound_leave_critical(cluster, CLUSTER_DOWN); + } else { + if (cpu_going_down) + platform_ops->cpu_powerdown_prepare(cpu, cluster); + arch_spin_unlock(&mcpm_lock); + /* + * If cpu_going_down is false here, that means a power_up + * request raced ahead of us. Even if we do not want to + * shut this CPU down, the caller still expects execution + * to return through the system resume entry path, like + * when the WFI is aborted due to a new IRQ or the like.. + * So let's continue with cache cleaning in all cases. + */ + platform_ops->cpu_cache_disable(); + } + + __mcpm_cpu_down(cpu, cluster); + + /* Now we are prepared for power-down, do it: */ + if (cpu_going_down) + wfi(); + + /* + * It is possible for a power_up request to happen concurrently + * with a power_down request for the same CPU. In this case the + * CPU might not be able to actually enter a powered down state + * with the WFI instruction if the power_up request has removed + * the required reset condition. We must perform a re-entry in + * the kernel as if the power_up method just had deasserted reset + * on the CPU. + */ + phys_reset = (phys_reset_t)(unsigned long)virt_to_phys(cpu_reset); + phys_reset(virt_to_phys(mcpm_entry_point)); + + /* should never get here */ + BUG(); +} + +int mcpm_wait_for_cpu_powerdown(unsigned int cpu, unsigned int cluster) +{ + int ret; + + if (WARN_ON_ONCE(!platform_ops || !platform_ops->wait_for_powerdown)) + return -EUNATCH; + + ret = platform_ops->wait_for_powerdown(cpu, cluster); + if (ret) + pr_warn("%s: cpu %u, cluster %u failed to power down (%d)\n", + __func__, cpu, cluster, ret); + + return ret; +} + +void mcpm_cpu_suspend(void) +{ + if (WARN_ON_ONCE(!platform_ops)) + return; + + /* Some platforms might have to enable special resume modes, etc. */ + if (platform_ops->cpu_suspend_prepare) { + unsigned int mpidr = read_cpuid_mpidr(); + unsigned int cpu = MPIDR_AFFINITY_LEVEL(mpidr, 0); + unsigned int cluster = MPIDR_AFFINITY_LEVEL(mpidr, 1); + arch_spin_lock(&mcpm_lock); + platform_ops->cpu_suspend_prepare(cpu, cluster); + arch_spin_unlock(&mcpm_lock); + } + mcpm_cpu_power_down(); +} + +int mcpm_cpu_powered_up(void) +{ + unsigned int mpidr, cpu, cluster; + bool cpu_was_down, first_man; + unsigned long flags; + + if (!platform_ops) + return -EUNATCH; + + mpidr = read_cpuid_mpidr(); + cpu = MPIDR_AFFINITY_LEVEL(mpidr, 0); + cluster = MPIDR_AFFINITY_LEVEL(mpidr, 1); + local_irq_save(flags); + arch_spin_lock(&mcpm_lock); + + cpu_was_down = !mcpm_cpu_use_count[cluster][cpu]; + first_man = mcpm_cluster_unused(cluster); + + if (first_man && platform_ops->cluster_is_up) + platform_ops->cluster_is_up(cluster); + if (cpu_was_down) + mcpm_cpu_use_count[cluster][cpu] = 1; + if (platform_ops->cpu_is_up) + platform_ops->cpu_is_up(cpu, cluster); + + arch_spin_unlock(&mcpm_lock); + local_irq_restore(flags); + + return 0; +} + +#ifdef CONFIG_ARM_CPU_SUSPEND + +static int __init nocache_trampoline(unsigned long _arg) +{ + void (*cache_disable)(void) = (void *)_arg; + unsigned int mpidr = read_cpuid_mpidr(); + unsigned int cpu = MPIDR_AFFINITY_LEVEL(mpidr, 0); + unsigned int cluster = MPIDR_AFFINITY_LEVEL(mpidr, 1); + phys_reset_t phys_reset; + + mcpm_set_entry_vector(cpu, cluster, cpu_resume); + setup_mm_for_reboot(); + + __mcpm_cpu_going_down(cpu, cluster); + BUG_ON(!__mcpm_outbound_enter_critical(cpu, cluster)); + cache_disable(); + __mcpm_outbound_leave_critical(cluster, CLUSTER_DOWN); + __mcpm_cpu_down(cpu, cluster); + + phys_reset = (phys_reset_t)(unsigned long)virt_to_phys(cpu_reset); + phys_reset(virt_to_phys(mcpm_entry_point)); + BUG(); +} + +int __init mcpm_loopback(void (*cache_disable)(void)) +{ + int ret; + + /* + * We're going to soft-restart the current CPU through the + * low-level MCPM code by leveraging the suspend/resume + * infrastructure. Let's play it safe by using cpu_pm_enter() + * in case the CPU init code path resets the VFP or similar. + */ + local_irq_disable(); + local_fiq_disable(); + ret = cpu_pm_enter(); + if (!ret) { + ret = cpu_suspend((unsigned long)cache_disable, nocache_trampoline); + cpu_pm_exit(); + } + local_fiq_enable(); + local_irq_enable(); + if (ret) + pr_err("%s returned %d\n", __func__, ret); + return ret; +} + +#endif + extern unsigned long mcpm_power_up_setup_phys; int __init mcpm_sync_init( @@ -249,8 +441,10 @@ } mpidr = read_cpuid_mpidr(); this_cluster = MPIDR_AFFINITY_LEVEL(mpidr, 1); - for_each_online_cpu(i) + for_each_online_cpu(i) { + mcpm_cpu_use_count[this_cluster][i] = 1; mcpm_sync.clusters[this_cluster].cpus[i].cpu = CPU_UP; + } mcpm_sync.clusters[this_cluster].cluster = CLUSTER_UP; sync_cache_w(&mcpm_sync);